{"cells":[{"metadata":{},"cell_type":"markdown","source":"https://arxiv.org/pdf/1812.05784.pdf"},{"metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true},"cell_type":"code","source":"# This Python 3 environment comes with many helpful analytics libraries installed\n# It is defined by the kaggle/python docker image: https://github.com/kaggle/docker-python\n# For example, here's several helpful packages to load in \n\nimport numpy as np # linear algebra\nimport pandas as pd # data processing, CSV file I/O (e.g. pd.read_csv)\n\n# Input data files are available in the \"../input/\" directory.\n# For example, running this (by clicking run or pressing Shift+Enter) will list all files under the input directory\n\nimport os\n!pip install -q lyft-dataset-sdk\n\n# Any results you write to the current directory are saved as output.","execution_count":null,"outputs":[]},{"metadata":{"_uuid":"d629ff2d2480ee46fbb7e2d37f6b5fab8052498a","_cell_guid":"79c7e3d0-c299-4dcb-8224-4455121ee9b0","trusted":true},"cell_type":"code","source":"import os\nimport math\n\nimport cv2\nimport numpy as np\nimport pandas as pd\nfrom pathlib import Path\n\n%matplotlib inline\nimport matplotlib.pyplot as plt\nimport matplotlib.patches as patches\nimport matplotlib as mpl\n\nfrom lyft_dataset_sdk.lyftdataset import LyftDataset\nfrom lyft_dataset_sdk.utils.data_classes import LidarPointCloud","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"INP_DIR = '/kaggle/input/3d-object-detection-for-autonomous-vehicles/'\n# Load the dataset\n# Adjust the dataroot parameter below to point to your local dataset path.\n# The correct dataset path contains at least the following four folders (or similar): images, lidar, maps, v1.0.1-train\n!ln -s {INP_DIR}/train_images images\n!ln -s {INP_DIR}/train_maps maps\n!ln -s {INP_DIR}/train_lidar lidar","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"level5data = LyftDataset(\n    data_path='.',\n    json_path=os.path.join(INP_DIR + 'train_data'),\n    verbose=False\n)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"my_scene = level5data.scene[0]\ntoken = my_scene['first_sample_token']\nsample = level5data.get('sample', token)\nlidar = level5data.get_sample_data(sample['data']['LIDAR_TOP'])\nlidar[1][3]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_coords_from_ann_idx(ann_idx, sample):\n    return np.array(level5data.get('sample_annotation', sample['anns'][ann_idx])['translation'])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"anns_inds_to_show = [0, 1, 2] # i select several cars near lyft's pod\nann_tokens = []\nfor ind in anns_inds_to_show:\n    my_annotation_token = sample['anns'][ind]\n    print(f'{ind}: {my_annotation_token}')\n    ann_tokens.append(my_annotation_token)\n    level5data.render_annotation(my_annotation_token)\n    plt.show()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"next_sample = level5data.get('sample', sample['next'])\nanns_inds_to_show = [0, 1, 2] # i select several cars near lyft's pod\nann_tokens = []\nfor ind in anns_inds_to_show:\n    my_annotation_token = next_sample['anns'][ind]\n    print(f'{ind}: {my_annotation_token}')\n    ann_tokens.append(my_annotation_token)\n    level5data.render_annotation(my_annotation_token)\n    plt.show()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"ltok = my_scene['last_sample_token']\nlevel5data.get('sample', ltok)['next']","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_data_from_sample(sample, channel_to_get):\n    return level5data.get('sample_data', sample['data'][channel_to_get])\nlidar_data = get_data_from_sample(sample, 'LIDAR_TOP')\nlidar_data_r = get_data_from_sample(sample, 'LIDAR_FRONT_RIGHT')\nlidar_data_l = get_data_from_sample(sample, 'LIDAR_FRONT_LEFT')\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"lidar_data = get_data_from_sample(sample, 'LIDAR_TOP')\npc = LidarPointCloud.from_file(Path(lidar_data['filename']))\nprint(pc.points.shape)\npc.points[:].max(axis=1), pc.points[:].min(axis=1), pc.points.mean(axis=1), pc.points.std(axis=1)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"level5data.get_boxes(sample['data']['LIDAR_TOP'])[32]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"level5data.get_sample_data(sample['data']['LIDAR_TOP'])[1][32]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"from collections import defaultdict\ndef sample_points(points, T=35, K=250):\n    points = points.T\n    points = points[:, :-1]\n    bounderies = [-40, -40, -3], [40, 40, 1]\n    points_in_volume = points[np.logical_and(np.less_equal(bounderies[0], points), np.less_equal(points, bounderies[1])).all(axis=-1)]\n    bounderies = np.array((-100, -100, -15)), np.array((100, 100, 5))\n    vd, vw, vh = 0.4, 0.4, 0.2\n    voxels = (points/np.array([vd, vw, vh]).reshape(1, 3)).round().astype(np.int)\n    voxel_dict = defaultdict(list)\n    for i, v in enumerate(voxels):\n        voxel_dict[tuple(v)].append(i)\n    voxel_dict = {k: points[np.random.choice(v, size=T)] for k, v in voxel_dict.items() \n                  if len(v) >= T and np.logical_and(bounderies[0] <= np.array(k), np.array(k) <= bounderies[1]).all()}\n    voxel_coords, voxel_features = zip(*list(voxel_dict.items())[:K])\n    voxel_features = np.stack(voxel_features)\n    voxel_coords = voxel_coords - bounderies[0]\n    pad_len = K - voxel_features.shape[0]\n    voxel_coords = np.pad(voxel_coords, [(0, pad_len), (0,0)], 'constant', constant_values=-1)\n    voxel_features = np.pad(voxel_features, [(0, pad_len), (0,0), (0,0)], 'constant', constant_values=0)\n    voxel_means = voxel_features.mean(axis=1, keepdims=True)\n    voxel_features = np.concatenate([voxel_features, voxel_features-voxel_means], axis=-1)\n    return voxel_features, voxel_coords\n    \nf, coords = sample_points(pc.points)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def gen_samples(n_scenes=1000):\n    for scene in level5data.scene[:n_scenes]:\n        sample = level5data.get('sample', scene['first_sample_token'])\n        while True:\n            yield sample\n            if sample['next']:\n                sample = level5data.get('sample', sample['next'])\n            else:\n                break\n    raise StopIteration\nfrom collections import defaultdict  \nclass Average:\n    def __init__(self, init=0.0):\n        self.n, self.sum = 0, init\n    \n    def __add__(self, v):\n        self.n += 1\n        self.sum += v\n        return self\n    \n    def __repr__(self):\n        return str(self.value)\n        \n    @property\n    def value(self):\n        if self.n==0:\n            return self.sum\n        return self.sum/self.n\n\nbox_wlh = defaultdict(lambda: Average(np.zeros(3)))\nbox_z_center = defaultdict(Average)\n#nums = defaultdict(int)\nfor sample in gen_samples(3000):\n    for bbox in level5data.get_sample_data(sample['data']['LIDAR_TOP'])[1]:\n        box_wlh[bbox.name] += bbox.wlh\n        box_z_center[bbox.name] += bbox.center[-1]\n        \nkeys = box_wlh.keys()\nbbox_shapes = np.array([box_wlh[k].value for k in keys])\nbbox_z_center = np.array([box_z_center[k].value for k in keys])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"cat2id = {cat['name']: i for i, cat in enumerate(level5data.category)}\nid2cat = {i: cat['name'] for i, cat in enumerate(level5data.category)}","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"bbox_shapes = np.array([box_wlh[cat['name']].value for cat in level5data.category])\nbbox_z_center = np.array([box_z_center[cat['name']].value for cat in level5data.category])\ndef create_anchors():\n    bounderies = np.array((-100, -100, -15)), np.array((100, 100, 5))\n    fm_w, fm_d = 96, 96\n    x_centers = np.linspace(bounderies[0][0], bounderies[1][0], fm_w, endpoint=False)\n    y_centers = np.linspace(bounderies[0][1], bounderies[1][1], fm_d, endpoint=False)\n    anchor_centers = np.stack(np.meshgrid(x_centers, y_centers), axis=-1)\n    anchor_centers = np.expand_dims(anchor_centers, 2)\n    n_bb = len(keys)*2\n    centers_xy = np.tile(anchor_centers, (1, 1, n_bb, 1))\n    centers_z = np.tile(bbox_z_center[np.newaxis,np.newaxis,:,np.newaxis], (fm_w,fm_d,2,1))\n    anchor_wlh = np.tile(bbox_shapes[np.newaxis,np.newaxis,:,:], (fm_w,fm_d,2,1))\n    anchor_angs = np.tile(np.array([0, np.pi/2])[np.newaxis,np.newaxis,:,np.newaxis], (fm_w,fm_d,len(keys),1))\n    return np.concatenate([centers_xy, centers_z, anchor_wlh, anchor_angs], axis=-1)\nanchors = create_anchors()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"boxes = []\nfor sample in gen_samples(3):\n    for bbox in level5data.get_sample_data(sample['data']['LIDAR_TOP'])[1]:\n        boxes.append(np.concatenate([bbox.center, bbox.wlh, np.array(bbox.orientation.yaw_pitch_roll[:1])]))\n    break\ntrue_boxes = np.array(boxes)\ntrue_boxes.shape","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_transform_matrix(x_center, y_center, w, d, theta):\n    return np.array([[w/2*np.cos(theta), d/2*np.sin(theta), x_center],\n                     [w/2*np.sin(theta), -d/2*np.cos(theta), y_center],\n                     [0, 0, 1]])\n    \ndef monte_carlo_overlap_2d(box1, box2 ,N=300):\n    x1,y1,z1,w1,d1,l1,r1 = box1\n    x2,y2,z2,w2,d2,l2,r2 = box2\n    to_box1 = get_transform_matrix(x1,y1,w1,d1,r1)\n    from_box2 = np.linalg.inv(get_transform_matrix(x2,y2,w2,d2,r2))\n    points = np.random.rand(2, N)*2 - 1 \n    points = np.concatenate([points, np.ones((1,N))], axis=0)\n    transformed_points = (from_box2@to_box1@points)[:-1]\n    in_box2 = np.all(np.logical_and(transformed_points < 1, -1 < transformed_points), axis=0)\n    return np.sum(in_box2)/N\n\ndef IOU_2d(box1, box2):\n    area1 = box1[3]*box1[4]\n    area2 = box2[3]*box2[4]\n    if False and area1 < area2:\n        box1, box2, area1, area2 = box2, box1, area2, area1\n    intersection = area1*monte_carlo_overlap_2d(box1, box2)\n    union = area1 + area2 - intersection\n    return intersection/union","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def bbox_overlap(anchors, gt_boxes):\n    anchors = anchors.reshape((-1, 7))\n    n, k = anchors.shape[0], gt_boxes.shape[0]\n    anchor_radii = np.linalg.norm(anchors[:,3:5], axis=-1)[:,np.newaxis]\n    boxes_radii = np.linalg.norm(gt_boxes[:,3:5], axis=-1)[np.newaxis,:]\n    sum_radii = anchor_radii + boxes_radii\n    center_distances = np.linalg.norm(anchors[:,np.newaxis,:2] - gt_boxes[np.newaxis,:,:2], axis=-1)\n    possible_overlaps = (center_distances < sum_radii)\n    possible_overlaps1 = np.abs(np.log(anchor_radii) - np.log(boxes_radii)) < np.log(2)\n    possible_overlaps = np.logical_and(possible_overlaps, possible_overlaps1)\n    iou = np.zeros((n,k))\n    for i in range(n):\n        for j in range(k):\n            if possible_overlaps[i,j]:\n                iou[i,j] = IOU_2d(anchors[i], gt_boxes[j])\n    return iou\n    \niou = bbox_overlap(anchors, true_boxes)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"mask = np.max(iou, axis=0)>0.35\nnp.arange(iou.shape[1])[mask], np.argmax(iou, axis=0)[mask]\nnp.where(iou > 0.5)\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"import torch\nfrom torch import nn\nfrom torch.utils.data import Dataset, DataLoader","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"cat2id = {cat['name']: i+1 for i, cat in enumerate(level5data.category)}\nid2cat = {i+1: cat['name'] for i, cat in enumerate(level5data.category)}\nbbox_shapes = np.array([box_wlh[cat['name']].value for cat in level5data.category])\nbbox_z_center = np.array([box_z_center[cat['name']].value for cat in level5data.category])\nclass lyft_data(Dataset):\n    T = 35 # מספר הנקודות שנדגמות מכל ווקסל\n    K = 250 # number of sampled voxels in a sample\n    vd, vw, vh = 0.4, 0.4, 0.2 # voxel sizes\n    voxel_dims = np.array([vd, vw, vh])\n    fm_w, fm_d = 96, 96 # feature map shape\n    n_anchors_per_position = 2*len(level5data.category) # 2 rotations (0, 90 degrees) * number of categories\n    pos_threshold, neg_threshold = 0.5, 0.35\n    bounderies = np.array([[-40, -40, -3],[40, 40, 1]])# \n    def __init__(self, samples):\n        self.samples = samples\n        self.anchors = create_anchors()\n        \n    def create_anchors():\n        x_centers = np.linspace(bounderies[0][0], bounderies[1][0], fm_w, endpoint=False)\n        y_centers = np.linspace(bounderies[0][1], bounderies[1][1], fm_d, endpoint=False)\n        anchor_centers = np.stack(np.meshgrid(x_centers, y_centers), axis=-1)\n        anchor_centers = np.expand_dims(anchor_centers, 2)\n        n_bb = self.n_anchors_per_position\n        centers_xy = np.tile(anchor_centers, (1, 1, n_bb, 1))\n        centers_z = np.tile(bbox_z_center[np.newaxis,np.newaxis,:,np.newaxis], (fm_w,fm_d,2,1))\n        anchor_wlh = np.tile(bbox_shapes[np.newaxis,np.newaxis,:,:], (fm_w,fm_d,2,1))\n        anchor_angs = np.tile(np.array([0, np.pi/2])[np.newaxis,np.newaxis,:,np.newaxis], (fm_w,fm_d,n_bb//2,1))\n        return np.concatenate([centers_xy, centers_z, anchor_wlh, anchor_angs], axis=-1)\n    \n    def __len__(self):\n        return len(self.samples)\n    \n    def __getitem__(self, i):\n        sample = self.samples[i]\n        lidar_tok = sample['data']['LIDAR_TOP']\n        bboxes = level5data.get_sample_data(lidar_tok)[1]\n        lidar_data = level5data.get('sample_data', lidar_tok)\n        pc = LidarPointCloud.from_file(Path(lidar_data['filename']))\n        voxel_features, voxel_coords = self.sample_points(pc.points) \n        pos, neg_equal_one, targets = self.cal_target(bboxes)\n        return voxel_features, voxel_coords, pos, neg_equal_one, targets\n        \n    def cal_target(self, gt_box3d):\n        # Input:\n        #   labels: (N,)\n        #   feature_map_shape: (w, l)\n        #   anchors: (w, l, 2, 7)\n        # Output:\n        #   pos_equal_one (w, l, 2)\n        #   neg_equal_one (w, l, 2)\n        #   targets (w, l, 14)\n        # attention: cal IoU on birdview\n        pos_equal_one = np.zeros((self.fm_w, self.fm_d, self.n_anchors_per_position))\n        neg_equal_one = np.zeros((self.fm_w, self.fm_d, self.n_anchors_per_position))\n        targets = np.zeros((self.fm_w, self.fm_d, 7*self.n_anchors_per_position))\n        \n        self.anchors = self.anchors.reshape((-1, 7))\n        \n        gt_xyzwlhr = np.stack([np.concatenate([bbox.center, bbox.wlh, np.array(bbox.orientation.yaw_pitch_roll[:1])])\\\n                               for bbox in gt_box3d], axis=0)\n        gt_categories = np.array([cat2id[bbox.name] for bbox in gt_box3d], dtype=np.int)\n        iou = bbox_overlap(self.anchors, gt_xyzwlhr)\n        \n        id_highest = np.argmax(iou, axis=0)  # the maximum anchor's ID\n        id_highest_gt = np.arange(iou.shape[1])\n        mask = iou[id_highest, id_highest_gt] > 0\n        id_highest, id_highest_gt = id_highest[mask], id_highest_gt[mask]\n        \n        # find anchor iou > cfg.XXX_POS_IOU\n        id_pos, id_pos_gt = np.where(iou > self.pos_threshold)\n        # find anchor iou < cfg.XXX_NEG_IOU\n        id_neg = np.where(np.sum(iou < self.neg_threshold, axis=1) == iou.shape[1])[0]\n        \n        id_pos = np.concatenate([id_pos, id_highest])\n        id_pos_gt = np.concatenate([id_pos_gt, id_highest_gt])\n\n        id_pos, index = np.unique(id_pos, return_index=True)\n        id_pos_gt = id_pos_gt[index]\n        category_gt = gt_categories[id_pos_gt]\n        id_neg.sort()\n        # cal the target and set the equal one\n        index_x, index_y, index_z = np.unravel_index(\n            id_pos, (self.fm_w, self.fm_d, self.n_anchors_per_position))\n        pos_equal_one[index_x, index_y, index_z] = category_gt\n        # ATTENTION: index_z should be np.array\n        \n        anchors_d = np.sqrt(self.anchors[:, 3]**2 + self.anchors[:, 4]**2)\n        targets[index_x, index_y, np.array(index_z) * 7] = \\\n            (gt_xyzwlhr[id_pos_gt, 0] - self.anchors[id_pos, 0]) / anchors_d[id_pos]\n        targets[index_x, index_y, np.array(index_z) * 7 + 1] = \\\n            (gt_xyzwlhr[id_pos_gt, 1] - self.anchors[id_pos, 1]) / anchors_d[id_pos]\n        targets[index_x, index_y, np.array(index_z) * 7 + 2] = \\\n            (gt_xyzwlhr[id_pos_gt, 2] - self.anchors[id_pos, 2]) / self.anchors[id_pos, 5]\n        targets[index_x, index_y, np.array(index_z) * 7 + 3] = np.log(\n            gt_xyzwlhr[id_pos_gt, 3] / self.anchors[id_pos, 3])\n        targets[index_x, index_y, np.array(index_z) * 7 + 4] = np.log(\n            gt_xyzwlhr[id_pos_gt, 4] / self.anchors[id_pos, 4])\n        targets[index_x, index_y, np.array(index_z) * 7 + 5] = np.log(\n            gt_xyzwlhr[id_pos_gt, 5] / self.anchors[id_pos, 5])\n        targets[index_x, index_y, np.array(index_z) * 7 + 6] = (\n                gt_xyzwlhr[id_pos_gt, 6] - self.anchors[id_pos, 6])\n        index_x, index_y, index_z = np.unravel_index(\n            id_neg, (self.fm_w, self.fm_d, self.n_anchors_per_position))\n        neg_equal_one[index_x, index_y, index_z] = 1\n        # to avoid a box be pos/neg in the same time\n        index_x, index_y, index_z = np.unravel_index(\n            id_highest, (self.fm_w, self.fm_d, self.n_anchors_per_position))\n        neg_equal_one[index_x, index_y, index_z] = 0\n\n        return pos_equal_one, neg_equal_one, targets\n        \n    \n    def sample_points(self, points):\n        points = points.T\n        points = points[:, :-1]\n        voxels = (points/self.voxel_dims.reshape(1, 3)).round().astype(np.int)\n        voxel_bounderies = self.bounderies/self.voxel_dims.reshape(1, 3)\n        voxel_dict = defaultdict(list)\n        for i, v in enumerate(voxels):\n            voxel_dict[tuple(v)].append(i)\n        i = 0\n        voxel_dict = {k: points[np.random.choice(v, size=self.T)] for k, v in voxel_dict.items() \n                      if len(v) >= self.T and np.logical_and(voxel_bounderies[0] <= np.array(k), np.array(k) <= voxel_bounderies[1]).all()}\n        voxel_coords, voxel_features = zip(*list(voxel_dict.items())[:self.K])\n        voxel_features = np.stack(voxel_features)\n        voxel_coords = voxel_coords - voxel_bounderies[0]\n        pad_len = self.K - voxel_features.shape[0]\n        voxel_coords = np.pad(voxel_coords, [(0, pad_len), (0,0)], 'constant', constant_values=-1)\n        voxel_features = np.pad(voxel_features, [(0, pad_len), (0,0), (0,0)], 'constant', constant_values=0)\n        voxel_means = voxel_features.mean(axis=1, keepdims=True)\n        voxel_features = np.concatenate([voxel_features, voxel_features-voxel_means], axis=-1)\n        return voxel_features, voxel_coords\n\n        ","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"data = lyft_data(list(gen_samples(3)))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"data[4]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pos_equal_one, neg_equal_one, targets = data.cal_target(level5data.get_sample_data(data.samples[20]['data']['LIDAR_TOP'])[1])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pos_equal_one.shape","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pos = pos_equal_one.reshape((96,96,9,2))\nnp.where(pos), pos[np.where(pos)]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"len([b.name for b in level5data.get_sample_data(data.samples[0]['data']['LIDAR_TOP'])[1]]), len(pos[np.where(pos)])","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}