{"cells":[{"metadata":{},"cell_type":"markdown","source":"# This notebook contains several visualizations where points are labeled by category of a particular bounding box"},{"metadata":{},"cell_type":"markdown","source":"Preprocessing based on https://www.kaggle.com/fartuk1/3d-segmentation-approach"},{"metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true},"cell_type":"code","source":"import numpy as np\nimport pandas as pd\nimport os\nprint(os.listdir('/kaggle/input/3d-object-detection-for-autonomous-vehicles'))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"!pip install pyquaternion","execution_count":null,"outputs":[]},{"metadata":{"_uuid":"d629ff2d2480ee46fbb7e2d37f6b5fab8052498a","_cell_guid":"79c7e3d0-c299-4dcb-8224-4455121ee9b0","trusted":true},"cell_type":"code","source":"import json\nimport os.path\n\nfrom matplotlib import pyplot as plt\nimport numpy as np\nimport pandas as pd\nfrom pyquaternion import Quaternion\n\nfrom mpl_toolkits.mplot3d import Axes3D\nimport random\nimport itertools\nfrom skimage.morphology import convex_hull_image","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"class Table:\n    def __init__(self, data):\n        self.data = data\n        self.index = {x['token']: x for x in data}\n\n\nDATA_ROOT = '/kaggle/input/3d-object-detection-for-autonomous-vehicles/'\n\n\ndef load_table(name, root=os.path.join(DATA_ROOT, 'train_data')):\n    with open(os.path.join(root, name), 'rb') as f:\n        return Table(json.load(f))\n\n    \nscene = load_table('scene.json')\nsample = load_table('sample.json')\nsample_data = load_table('sample_data.json')\nego_pose = load_table('ego_pose.json')\ncalibrated_sensor = load_table('calibrated_sensor.json')","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"train_df = pd.read_csv(os.path.join(DATA_ROOT, 'train.csv')).set_index('Id')\nCLASSES = [\"car\", \"motorcycle\", \"bus\", \"bicycle\", \"truck\", \"pedestrian\", \"other_vehicle\", \"animal\", \"emergency_vehicle\"]","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"Translations of coordinates from https://www.kaggle.com/lopuhin/lyft-3d-join-all-lidars-annotations-from-scratch"},{"metadata":{"trusted":true},"cell_type":"code","source":"def rotate_points(points, rotation, inverse=False):\n    assert points.shape[1] == 3\n    q = Quaternion(rotation)\n    if inverse:\n        q = q.inverse\n    return np.dot(q.rotation_matrix, points.T).T\n    \ndef apply_pose(points, cs, inverse=False):\n    \"\"\" Translate (lidar) points to vehicle coordinates, given a calibrated sensor.\n    \"\"\"\n    points = rotate_points(points, cs['rotation'])\n    points = points + np.array(cs['translation'])\n    return points\n\ndef inverse_apply_pose(points, cs):\n    \"\"\" Reverse of apply_pose (we'll need it later).\n    \"\"\"\n    points = points - np.array(cs['translation']) \n    points = rotate_points(points, np.array(cs['rotation']), inverse=True)\n    return points\n\ndef get_annotations(token):\n    annotations = np.array(train_df.loc[token].PredictionString.split()).reshape(-1, 8)\n    return {\n        'point': annotations[:, :3].astype(np.float32),\n        'wlh': annotations[:, 3:6].astype(np.float32),\n        'rotation': annotations[:, 6].astype(np.float32),\n        'cls': np.array(annotations[:, 7]),\n    }","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"Helpers to rotate bounding box points"},{"metadata":{"trusted":true},"cell_type":"code","source":"import copy\n\nimport math\n\ndef rotate(origin, point, angle):\n    ox, oy, _ = origin\n    px, py, pz = point\n\n    qx = ox + math.cos(angle) * (px - ox) - math.sin(angle) * (py - oy)\n    qy = oy + math.sin(angle) * (px - ox) + math.cos(angle) * (py - oy)\n    return [qx, qy, pz]\n\n\ndef make_box_coords(center, wlh, rotation, ep):\n\n    planar_wlh = copy.deepcopy(wlh)\n    planar_wlh = planar_wlh[[1,0,2]]\n\n    bottom_center = copy.deepcopy(center)\n    bottom_center[-1] = bottom_center[-1] - planar_wlh[-1] / 2\n\n    bottom_points = []\n    bottom_points.append(bottom_center + planar_wlh * [1, 1, 0] / 2)\n    bottom_points.append(bottom_center + planar_wlh * [-1, -1, 0] / 2)\n    bottom_points.append(bottom_center + planar_wlh * [1, -1, 0] / 2)\n    bottom_points.append(bottom_center + planar_wlh * [-1, 1, 0] / 2)\n    bottom_points = np.array(bottom_points)\n\n    rotated_bottom_points = []\n    for point in bottom_points:\n        rotated_bottom_points.append(rotate(bottom_center, point, rotation))\n\n    rotated_bottom_points = np.array(rotated_bottom_points)\n    rotated_top_points = rotated_bottom_points + planar_wlh * [0,0,1]\n\n    box_points = np.concatenate([rotated_bottom_points, rotated_top_points], axis=0)\n\n    box_points = inverse_apply_pose(box_points, ep)\n    \n    return box_points","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_sample_data(sample_token):\n    lidars = []\n    for x in sample_data.data:\n        if x['sample_token'] == sample_token and 'lidar' in x['filename']:\n            print(x['filename'])\n            lidars.append(x)\n\n    lidars_data = [\n        # here, sorry\n        np.fromfile(os.path.join(DATA_ROOT, x['filename'].replace('lidar/', 'train_lidar/')), dtype=np.float32)\n        .reshape(-1, 5)[:, :3] for x in lidars]\n\n\n    all_points = []\n    for points, lidar in zip(lidars_data, lidars):\n        cs = calibrated_sensor.index[lidar['calibrated_sensor_token']]\n        points = apply_pose(points, cs)\n        all_points.append(points)\n    all_points = np.concatenate(all_points)\n\n\n    ego_pose_token, = {x['ego_pose_token'] for x in lidars}\n    ep = ego_pose.index[ego_pose_token]\n    annotations = get_annotations(sample_token)\n    \n    all_boxes = {}\n    for class_name in CLASSES:\n        obj_centers = annotations['point'][annotations['cls'] == class_name]\n        obj_wlhs = annotations['wlh'][annotations['cls'] == class_name]\n        obj_rotations = annotations['rotation'][annotations['cls'] == class_name]\n        obj_boxes = []\n        for k in range(len(obj_centers)):\n            center = obj_centers[k]\n            wlh = obj_wlhs[k]\n            rotation = obj_rotations[k]\n            box_coords = make_box_coords(center, wlh, rotation, ep)\n            obj_boxes.append(box_coords)\n        all_boxes[class_name] = np.array(obj_boxes)\n        \n    car_centers = annotations['point'][annotations['cls'] == 'car'] \n    car_centers = inverse_apply_pose(car_centers, ep)\n    return all_points, all_boxes","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"## Data example"},{"metadata":{"trusted":true},"cell_type":"code","source":"train_df","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"sample_token = train_df.reset_index()['Id'].values[20]\nall_points, all_boxes = get_sample_data(sample_token)\nprint(all_points.shape)\nfor obj in all_boxes:\n    print(obj, all_boxes[obj].shape)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# plot top view with bbox corners\nplt.figure(figsize=(25,15))\nplt.scatter(all_points[:, 0], all_points[:, 1],s=[0.1]*len(all_points))\nfor obj in all_boxes:\n    if all_boxes[obj].shape[0] == 0:\n        continue\n    boxes_coords = np.concatenate(all_boxes[obj], axis=0)\n    plt.scatter(boxes_coords[:, 0], boxes_coords[:, 1],s=[15]*len(boxes_coords))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def points_in_cuboid(bbox, cloud, bbox_idx=[0, 2, 3, 4]):\n    \"\"\"Get points within rectangular cuboid. Make sure the cuboid coordinates are in correct order.\n    \n    bbox: 8 points as corneres of cuboid.\n    bbox_idx: indices of bbox which should be taken for vector creation.\n            First and last index are bottom and upper points on the same edge. \n            2nd and 3th index are bottom points, but not diagonal to 1st index.\n    \"\"\"\n    i = bbox[bbox_idx[1]] - bbox[bbox_idx[0]]\n    j = bbox[bbox_idx[2]] - bbox[bbox_idx[0]]\n    k = bbox[bbox_idx[3]] - bbox[bbox_idx[0]]\n    v_s = cloud - bbox[bbox_idx[0]]\n    ii_dot = np.dot(i, i) \n    jj_dot = np.dot(j, j) \n    kk_dot = np.dot(k, k) \n    vi_dot = np.dot(v_s, i)\n    vj_dot = np.dot(v_s, j) \n    vk_dot = np.dot(v_s, k)\n    mask = (0 <= vi_dot) & (vi_dot <= ii_dot) & (0 <= vj_dot) & (vj_dot <= jj_dot) & (0 <= vk_dot) & (vk_dot <= kk_dot)\n    return mask","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def plot_small_region_masked(ann_idx):\n    center_point = all_boxes['car'][ann_idx].mean(axis=0)\n    x_min = center_point[0] - 5\n    x_max = center_point[0] + 5\n    y_min = center_point[1] - 5\n    y_max = center_point[1] + 5\n    z_min= center_point[2] - 5\n    z_max = center_point[2] + 5\n\n    area_mask = (all_points[:, 0] > x_min) * (all_points[:, 0] < x_max) * (all_points[:, 1] > y_min) * (all_points[:, 1] < y_max) * (all_points[:, 2] > z_min) * (all_points[:, 2] < z_max)\n    area_mask = np.where(area_mask)[0]\n\n    fig = plt.figure(figsize=(25,15))\n    ax = Axes3D(fig)\n    \n    mask_in = points_in_cuboid(all_boxes['car'][ann_idx], all_points[area_mask, 0:3], bbox_idx=[0, 2, 3, 4])\n    \n    ax.scatter(all_points[area_mask, 0][mask_in], all_points[area_mask, 1][mask_in], all_points[area_mask, 2][mask_in])\n    ax.scatter(all_points[area_mask, 0][~mask_in], all_points[area_mask, 1][~mask_in], all_points[area_mask, 2][~mask_in])\n\n    ax.scatter(all_boxes['car'][ann_idx][:, 0], all_boxes['car'][ann_idx][:, 1], all_boxes['car'][ann_idx][:, 2], color='r', s=[100])\n    plt.show()","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Small regions around bbox of a car class\nBlue - points inside chosen bbox, orange - points outside chosen bbox, red - bbox corners"},{"metadata":{"trusted":true},"cell_type":"code","source":"for i in range(all_boxes['car'].shape[0]):\n    plot_small_region_masked(i)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_points_inside_boxes(points, boxes):\n    \"\"\"return mask of all points which are in any bbox \"\"\"\n    all_masks = []\n    for box_i in range(boxes.shape[0]):\n        mask_in = points_in_cuboid(boxes[box_i], points, bbox_idx=[0, 2, 3, 4])\n        all_masks.append(mask_in)\n    return np.array(all_masks).any(axis=0)","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"## Examples of labeled surrounding points\nSome plots use more lidar sensors, so they are more dense, especially first one looks noisy on this figure."},{"metadata":{"trusted":true},"cell_type":"code","source":"colors = ['#CBD6F166', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']\nmap_dict = { i: color for i, color in enumerate(colors)}\n\nimport matplotlib.patches as mpatches\n\n\nfor i in range(0,train_df.shape[0], 1000):\n    print(i)\n    sample_token = train_df.reset_index()['Id'].values[i]\n    all_points, all_boxes = get_sample_data(sample_token)\n\n    seg_lables = np.zeros((all_points.shape[0],))\n    for i, (obj, boxes) in enumerate(all_boxes.items()):\n        if not boxes.shape[0] == 0:\n            mask_in = get_points_inside_boxes(all_points, boxes)\n            seg_lables[mask_in] = i+1\n\n\n    fig = plt.figure(figsize=(25,15))\n    ax = Axes3D(fig)\n    mask = (all_points[:, 0] < 20) & (all_points[:, 0] > -20) & (all_points[:, 1] < 20) & (all_points[:, 1] > -20)\n    ax.scatter(all_points[mask, 0], all_points[mask, 1], all_points[mask, 2], c=np.vectorize(map_dict.get)(seg_lables[mask]))\n    ax.legend()\n    ax.set_zlim(-6, 6)\n    \n    patches = []\n    for col, cla in zip(colors, ['']+CLASSES):\n        patch = mpatches.Patch(color=col, label=cla)\n        patches.append(patch)\n    plt.legend(handles=patches)\n        \n    plt.show()","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"## Create labels for all points in train dataset\nwork in progress"},{"metadata":{"trusted":true},"cell_type":"code","source":"# points_train = []\n# seg_labels_train = []\n# for i in range(100):\n#     sample_token = train_df.reset_index()['Id'].values[i]\n#     all_points, all_boxes, car_centers = get_sample_data(sample_token)\n\n#     seg_lables = np.zeros((all_points.shape[0],))\n#     for i, (obj, boxes) in enumerate(all_boxes.items()):\n#         if not boxes.shape[0] == 0:\n#             mask_in = get_points_inside_boxes(all_points, boxes)\n#             seg_lables[mask_in] = i+1\n    \n#     points_train.append(all_points)\n#     seg_labels_train.append(seg_lables)","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}