{"cells":[{"metadata":{"trusted":true},"cell_type":"code","source":"!pip install lyft-dataset-sdk","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"!pip install squaternion","execution_count":null,"outputs":[]},{"metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true},"cell_type":"code","source":"import pdb\nimport cv2\nimport os\nfrom PIL import Image, ImageDraw, ImageFont\nfrom matplotlib import pyplot as plt\nfrom pathlib import Path\nimport pandas as pd\nimport numpy as np\nimport torch\nimport torch.utils.data\nimport torch.nn as nn\nfrom torch.autograd import Variable\nimport torch.optim as optim\nimport torch.nn.functional as F\n\n# Load the SDK\nfrom lyft_dataset_sdk.lyftdataset import LyftDataset, LyftDatasetExplorer\nfrom lyft_dataset_sdk.utils.data_classes import Box, LidarPointCloud, RadarPointCloud \nfrom lyft_dataset_sdk.utils.geometry_utils import BoxVisibility, box_in_image, view_points\nfrom lyft_dataset_sdk.eval.detection.mAP_evaluation import Box3D, recall_precision\nimport math\n\n# from source.utilities import print_progress\nfrom tqdm import tqdm_notebook as tqdm\n\n%matplotlib inline\nimport string \nfrom pyquaternion import Quaternion\nimport matplotlib.patches as patches\n\nfrom squaternion import euler2quat, quat2euler\nimport time\nfrom multiprocessing import Process\nfrom sklearn.decomposition import PCA","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# gotta do this for LyftDataset SDK, it expects folders to be named as `images`, `maps`, `lidar`\n\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\n!ln -s /kaggle/input/3d-object-detection-for-autonomous-vehicles/train_data data","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"level5data = LyftDataset(data_path='.', json_path='data/', verbose=True)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"train_pred = pd.read_csv('../input/lyft3d-inference-kernel-train-dataset/lyft3d_pred_train.csv')\ntrain_pred.head()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_pred_boxes_list_3d(pred_boxes):\n    if not pd.isna(pred_boxes):\n        pred_boxes = pred_boxes.split(' ')\n        pred_boxes = pred_boxes[:-1]\n        pred_boxes_list = np.reshape(pred_boxes, (len(pred_boxes)//9, 9))\n    else:\n        pred_boxes_list = []\n    return pred_boxes_list","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pred_boxes = train_pred.iloc[0]['PredictionString']\npred_boxes_list = get_pred_boxes_list_3d(pred_boxes)\npred_boxes_list","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"train = pd.read_csv('../input/different-classes/pred_animal_3D.csv')\ntrain.head()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_gth_boxes_list_3d(pred_boxes):\n    if not pd.isna(pred_boxes):\n        pred_boxes = pred_boxes.split(' ')\n        pred_boxes = pred_boxes[1:-1]\n        pred_boxes_list = np.reshape(pred_boxes, (len(pred_boxes)//10, 10))\n    else:\n        pred_boxes_list = []\n    return pred_boxes_list","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pred_boxes = train.iloc[0]['3D']\ngth_list = get_gth_boxes_list_3d(pred_boxes)\ngth_list","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"df_pred = pd.read_csv(\"../input/rcnn-pytorch-animal-detection/rcnn_pred_animal_2d.csv\")\ndf_pred.head()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_boxes_list(pred_boxes):\n    pred_boxes = pred_boxes.split(' ')\n    pred_boxes_list = pred_boxes[:-1]\n    pred_boxes_list = np.reshape(pred_boxes_list, (len(pred_boxes_list)//6, 6))\n    return pred_boxes_list","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"title = ['CAM_FRONT_LEFT', 'CAM_FRONT', 'CAM_FRONT_RIGHT', 'CAM_BACK_LEFT', 'CAM_BACK', 'CAM_BACK_RIGHT', 'CAM_FRONT_ZOOMED']","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"train_num = len(train)\ntotal_num_pred = 0\ntotal_num = 0\nfor idx in tqdm(range(train_num)):\n    total_num += int(train.iloc[idx]['3D'][0])\n    for i in range(7):\n        pred_boxes = df_pred.iloc[idx][title[i]]\n        if not pd.isna(pred_boxes):\n            pred_boxes_list = get_boxes_list(pred_boxes)\n            num = pred_boxes_list.shape[0]\n            total_num_pred += num","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"print(total_num, total_num_pred, total_num_pred/total_num)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"columns = ['Id', '2D']\ndf_pred_2d = pd.DataFrame(columns=columns)\ndf_pred_2d.head()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"train_num = len(train)\nfor idx in tqdm(range(train_num)):\n    token = train.iloc[idx]['Id']\n    row = []\n    row.append(token)\n    result = ''\n    for i in range(7):\n        pred_boxes = df_pred.iloc[idx][title[i]]\n        if not pd.isna(pred_boxes):\n            pred_boxes_list = get_boxes_list(pred_boxes)\n            x1 = pred_boxes_list[0][0]\n            y1 = pred_boxes_list[0][1]\n            x2 = pred_boxes_list[0][2]\n            y2 = pred_boxes_list[0][3]\n            cls_conf = pred_boxes_list[0][4]\n            label = pred_boxes_list[0][5]\n            cam = title[i]\n            result += str(x1) + ' ' + str(y1) + ' ' + str(x2) + ' ' + str(y2) + ' ' + str(cls_conf) + ' ' + str(label) + ' '  + str(cam) + \" \"\n    row.append(result)  \n    if not result == '':\n        df_row = pd.DataFrame([row], columns=columns)\n        df_pred_2d = df_pred_2d.append(df_row)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"df_pred_2d.shape[0]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"df_pred_2d.head()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def detection_show(pred_boxes_list):\n    for pred_box in pred_boxes_list:\n        coor = pred_box[:4]\n        x1 = int(float(coor[0]))\n        y1 = int(float(coor[1]))\n        x2 = int(float(coor[2]))\n        y2 = int(float(coor[3]))\n        cls_conf = pred_box[4]\n        label = pred_box[5]\n        cam = pred_box[6]\n        photo_filename = level5data.get('sample_data', my_sample['data'][cam])['filename']\n        img = cv2.imread(photo_filename) # Read image with cv2\n        img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) # Convert to RGB   \n        cv2.rectangle(img, (x1, y1),(x2, y2),255, 3) # Draw Rectangle with the coordinates\n        cv2.putText(img, cls_conf, (x1, y1),  cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0),thickness=3) # Write the prediction class\n        plt.figure(figsize=(15,10)) # display the output image\n        plt.imshow(img)\n        plt.xticks([])\n        plt.yticks([])\n        plt.show()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_2d_boxes_list(pred_boxes):\n    pred_boxes = pred_boxes[:-1]\n    pred_boxes = pred_boxes.split(' ')\n    pred_boxes_list = np.reshape(pred_boxes, (len(pred_boxes)//7, 7))\n    return pred_boxes_list","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"train_num = len(df_pred_2d)\nfor idx in range(10):\n    token = df_pred_2d.iloc[idx]['Id']\n    my_sample = level5data.get('sample', token)\n    pred_boxes = df_pred_2d.iloc[idx]['2D']\n    pred_boxes_list = get_2d_boxes_list(pred_boxes)\n    detection_show(pred_boxes_list)\n    print(idx)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"class_heights = {'animal':0.51,'bicycle':1.44,'bus':3.44,'car':1.72,'emergency_vehicle':2.39,'motorcycle':1.59,\n                'other_vehicle':3.23,'pedestrian':1.78,'truck':3.44}\nclass_lens = {'animal':0.73,'bicycle':1.76,'bus':12.34,'car':4.76,'emergency_vehicle':6.52,'motorcycle':2.35,\n                'other_vehicle':8.20,'pedestrian':0.81,'truck':10.24}\nclass_widths = {'animal':0.36,'bicycle':0.63,'bus':2.96,'car':1.93,'emergency_vehicle':2.45,'motorcycle':0.96,\n                'other_vehicle':2.79,'pedestrian':0.77,'truck':2.84}","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pred_boxes = df_pred_2d.iloc[18]['2D']\npred_boxes_list = get_2d_boxes_list(pred_boxes)\nprint(pred_boxes_list)\nfor pred_box in pred_boxes_list:\n    coor = pred_box[:4]\n    x1 = int(float(coor[0]))\n    y1 = int(float(coor[1]))\n    x2 = int(float(coor[2]))\n    y2 = int(float(coor[3]))\n    cls_conf = pred_box[4]\n    label = pred_box[5]\n    cam = pred_box[6]\n    print(x1)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def project_2D_to_3D(points, Z, intrinsic, quaternion, translation):\n    f_x = intrinsic[0, 0]\n    f_y = intrinsic[1, 1]\n    c_x = intrinsic[0, 2]\n    c_y = intrinsic[1, 2]\n    result = []\n    for idx in range(points.shape[0]):\n        point = []\n        z = Z\n        x = z * (points[idx, 0] - c_x) / f_x \n        y = z * (points[idx, 1] - c_y) / f_y\n        point.append([x, y, z])\n        point_rot = np.dot(quaternion.rotation_matrix, point[0]) + np.array(translation)\n        result.append(point_rot)\n    return result","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def render_pointcloud_xyz(point_cloud, color_channel, ax, x_lim, y_lim, marker_size):\n    colors = np.ones(len(point_cloud[:,2]))*100\n    ax.scatter(point_cloud[:, 0], point_cloud[:, 1], c = colors, s=marker_size)\n    ax.set_xlim(x_lim[0], x_lim[1])\n    ax.set_ylim(y_lim[0], y_lim[1]) ","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def render_pointcloud_xz(point_cloud, color_channel, ax, x_lim, z_lim, marker_size):\n    colors = np.ones(len(point_cloud[:,2]))*100\n    ax.scatter(point_cloud[:, 0], point_cloud[:, 2], c = colors, s=marker_size)\n    ax.set_xlim(x_lim[0], x_lim[1])\n    ax.set_ylim(z_lim[0], z_lim[1]) ","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def render_pointcloud_yz(point_cloud, color_channel, ax, y_lim, z_lim, marker_size):\n    colors = np.ones(len(point_cloud[:,2]))*100\n    ax.scatter(point_cloud[:, 1], point_cloud[:, 2], c = colors, s=marker_size)\n    ax.set_xlim(y_lim[0], y_lim[1])\n    ax.set_ylim(z_lim[0], z_lim[1]) ","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"pred_box3ds = []\npred_box_point_cloud = []\nnum_samples = len(df_pred_2d)\nfor idx in tqdm(range(num_samples)):\n    token0 = df_pred_2d.iloc[idx]['Id']\n    my_sample = level5data.get('sample', token0)\n    pred_boxes = df_pred_2d.iloc[idx]['2D']\n    pred_boxes_list = get_2d_boxes_list(pred_boxes)\n    #print(token0, pred_boxes_list)\n    for pred_box in pred_boxes_list:\n        coor = pred_box[:4]\n        x1 = int(float(coor[0]))\n        y1 = int(float(coor[1]))\n        x2 = int(float(coor[2]))\n        y2 = int(float(coor[3]))\n        cls_conf = float(pred_box[4])\n        if cls_conf < 0.7:\n            break\n        cls_pred = pred_box[5]\n        cam_type = pred_box[6]\n        box_h = abs(y2 - y1)\n        box_w = abs(x2 - x1)\n        #print(cam)\n        sample_cam_token = my_sample[\"data\"][cam_type]\n        data_path, boxes, camera_intrinsic = level5data.get_sample_data(sample_cam_token, box_vis_level=BoxVisibility.ANY)\n        sample_lidar_token = my_sample[\"data\"][\"LIDAR_TOP\"]\n        pcl_path, boxes, lidar_intrinsic = level5data.get_sample_data(sample_lidar_token)\n        pointsensor = level5data.get(\"sample_data\", sample_lidar_token)\n        pc = LidarPointCloud.from_file(pcl_path)\n        #print(pcl_path)\n\n        # Points live in the point sensor frame. So they need to be transformed via global to the image plane.\n        # First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep.\n        cs_record = level5data.get(\"calibrated_sensor\", pointsensor[\"calibrated_sensor_token\"])\n        pc.rotate(Quaternion(cs_record[\"rotation\"]).rotation_matrix)\n        pc.translate(np.array(cs_record[\"translation\"]))\n\n        # Second step: transform to the global frame.\n        poserecord = level5data.get(\"ego_pose\", pointsensor[\"ego_pose_token\"])\n        pc.rotate(Quaternion(poserecord[\"rotation\"]).rotation_matrix)\n        pc.translate(np.array(poserecord[\"translation\"]))\n\n        cam = level5data.get(\"sample_data\", sample_cam_token)\n        # Third step: transform into the ego vehicle frame for the timestamp of the image.\n        poserecord = level5data.get(\"ego_pose\", cam[\"ego_pose_token\"])\n        pc.translate(-np.array(poserecord[\"translation\"]))\n        pc.rotate(Quaternion(poserecord[\"rotation\"]).rotation_matrix.T)\n\n        # Fourth step: transform into the camera.\n        cs_record = level5data.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n        pc.translate(-np.array(cs_record[\"translation\"]))\n        pc.rotate(Quaternion(cs_record[\"rotation\"]).rotation_matrix.T)\n\n        # Fifth step: actually take a \"picture\" of the point cloud.\n        # Grab the depths (camera frame z axis points away from the camera).\n        depths = pc.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(pc.points[:3, :], np.array(cs_record[\"camera_intrinsic\"]), normalize=True)\n\n        im = Image.open(str(level5data.data_path / cam[\"filename\"]))\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, :] < im.size[0] - 1)\n        mask = np.logical_and(mask, points[1, :] > 1)\n        mask = np.logical_and(mask, points[1, :] < im.size[1] - 1)\n\n        row_mask1 = np.logical_and(points[0, :] >= x1, points[0, :] <= x2)\n        row_mask2 = np.logical_and(points[1, :] >= y1, points[1, :] <= y2)\n        row_mask = np.logical_and(row_mask1, row_mask2)\n\n        input_2Dbbox = np.array([x1, x2, y1, y2])\n        base_points_cam = []\n        base_points_cam.append((x1, y2, 1))\n        base_points_cam.append((x2, y2, 1))\n        base_points_cam = np.array(base_points_cam)\n        cs_record = level5data.get(\"calibrated_sensor\", cam[\"calibrated_sensor_token\"])\n        quaternion = Quaternion(cs_record[\"rotation\"])\n        translation = cs_record[\"translation\"]\n        for i in range(80):\n            points_projected = project_2D_to_3D(base_points_cam, i, camera_intrinsic, quaternion, translation)\n            z = points_projected[0][2]\n            if z <= 0:\n                break\n              \n        #print(i, cam)\n        #z1 = i-1\n        #z2 = z1 + 1\n\n\n\n        pc0 = LidarPointCloud.from_file(pcl_path)\n        pointsensor_token = sample_lidar_token\n        pointsensor = level5data.get(\"sample_data\", pointsensor_token)\n        cs_record = level5data.get(\"calibrated_sensor\", pointsensor[\"calibrated_sensor_token\"])\n        pc0.rotate(Quaternion(cs_record[\"rotation\"]).rotation_matrix)\n        pc0.translate(np.array(cs_record[\"translation\"]))\n        \n        # Second step: transform to the global frame.\n        poserecord = level5data.get(\"ego_pose\", pointsensor[\"ego_pose_token\"])\n        pc0.rotate(Quaternion(poserecord[\"rotation\"]).rotation_matrix)\n        pc0.translate(np.array(poserecord[\"translation\"]))\n\n        cam = level5data.get(\"sample_data\", sample_cam_token)\n        # Third step: transform into the ego vehicle frame for the timestamp of the image.\n        poserecord = level5data.get(\"ego_pose\", cam[\"ego_pose_token\"])\n        pc0.translate(-np.array(poserecord[\"translation\"]))\n        pc0.rotate(Quaternion(poserecord[\"rotation\"]).rotation_matrix.T)\n        \n        \n        point_cloud = pc0.points.transpose((1, 0))\n\n        #depth_mask = np.logical_and((pc0.points[0, :]**2 + pc0.points[1, :]**2) >= z1**2, \\\n        #                            (pc0.points[0, :]**2 + pc0.points[1, :]**2) <= z2**2)\n\n        mask = np.logical_and(mask, row_mask)\n        #all_mask = np.logical_and(mask, depth_mask)\n\n        box_point_cloud = point_cloud[mask, :]\n        \n        if box_point_cloud.shape[0] > 0:\n            z_box_center = np.mean(box_point_cloud[:, 2])\n            mask = np.ones(box_point_cloud.shape[0], dtype=bool)\n            h_mask = np.logical_and(mask, box_point_cloud[:, 2] > (z_box_center+0.1))\n            h_point_cloud = box_point_cloud[h_mask,:]\n            if h_point_cloud.shape[0] > 0:\n                x_box_center = np.mean(h_point_cloud[:,0])\n                y_box_center = np.mean(h_point_cloud[:,1])\n                box_mask1 = np.logical_and(box_point_cloud[:, 0]>=(x_box_center-1), box_point_cloud[:, 0]<=(x_box_center+1))\n                box_mask2 = np.logical_and(box_point_cloud[:, 1]>=(y_box_center-1), box_point_cloud[:, 1]<=(y_box_center+1)) \n                box_mask = np.logical_and(box_mask1, box_mask2)\n                box_point_cloud = box_point_cloud[box_mask,:]\n                if box_point_cloud.shape[0] > 2:\n                    pca = PCA(n_components=2)\n                    pca.fit(box_point_cloud[:,:2])\n                    vector = pca.components_[0]\n                    yaw = np.arctan(vector[1]/vector[0])\n                    q = euler2quat(0, 0, yaw)\n                    pred_box_point_cloud.append([token0,cam_type, box_point_cloud])\n                    x_box_center = np.mean(box_point_cloud[:,0])\n                    y_box_center = np.mean(box_point_cloud[:, 1])\n                    #z_box_center = np.mean(box_point_cloud[:, 2])\n                    #z_box_center = min(box_point_cloud[:, 2]) + class_heights[cls_pred]/2\n                    \n                    dist = np.sqrt(x_box_center*x_box_center + y_box_center*y_box_center)\n                    points_projected = project_2D_to_3D(base_points_cam, dist, camera_intrinsic, quaternion, translation)\n                    l1 = abs(points_projected[0][0] - points_projected[1][0])\n                    l2 = abs(points_projected[0][1] - points_projected[1][1])\n                    l = np.sqrt(l1**2 + l2**2)\n                    w = 0.5 * l\n                    h = abs(0.8 * l * (y2-y1)/(x2-x1)) \n                    z_box_center = np.mean(box_point_cloud[:, 2])\n                    #print(box_center)\n                    #w = class_widths[cls_pred]\n                    #h = class_heights[cls_pred]\n                    #l = class_lens[cls_pred]\n                    box_wlh = [w, l, h]\n                    box_center = [x_box_center, y_box_center, z_box_center]\n                    q = Quaternion(w=q[0], x=q[1], y=q[2], z=q[3])\n                    box1 = Box(\n                           center=box_center,\n                           size=box_wlh,\n                           orientation=q,\n                           name=cls_pred,\n                           score=cls_conf,\n                           token=token0\n                        )\n\n\n                    poserecord = level5data.get(\"ego_pose\", cam[\"ego_pose_token\"])\n                    box1.rotate(Quaternion(poserecord[\"rotation\"]))\n                    box1.translate(np.array(poserecord[\"translation\"]))\n                    q = box1.orientation.elements\n                    box_center = box1.center\n\n                    box3d = Box3D(\n                            sample_token=token0,\n                            translation=box_center,\n                            size=box_wlh,\n                            rotation=[q[0],q[1],q[2],q[3]],\n                            name=cls_pred,\n                            score=cls_conf\n                        )\n\n                    pred_box3ds.append(box3d)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"len(pred_box3ds)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"len(pred_box_point_cloud)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def draw_vector(v0, v1, ax=None):\n    ax = ax or plt.gca()\n    arrowprops=dict(arrowstyle='->',linewidth=2,shrinkA=0, shrinkB=0)\n    ax.annotate('', v1, v0, arrowprops=arrowprops)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def show_lidar_cloud(box_list_3d_pred, pred_box3ds, point_cloud, sample_lidar_token, token, cam): \n    fig, ax = plt.subplots(1,3, figsize=(18, 6))\n    \n    sample_cam_token = my_sample[\"data\"][cam]\n    sd_record = level5data.get(\"sample_data\", sample_cam_token)\n    cs_record = level5data.get(\"calibrated_sensor\", sd_record[\"calibrated_sensor_token\"])\n    sensor_record = level5data.get(\"sensor\", cs_record[\"sensor_token\"])\n    pose_record = level5data.get(\"ego_pose\", sd_record[\"ego_pose_token\"])\n    #print(pose_record)\n    \n\n    print(\"predicted from 3d boxes: \") \n    for box in box_list_3d_pred:\n        x, y, z = box.center\n        w, l, h = box.wlh\n        name = box.name\n        if name == 'animal':\n            x_min = x - l/2\n            y_min = y - w/2\n            rect = patches.Rectangle((x_min,y_min),l,w,linewidth=3,edgecolor='r',facecolor='none')\n\n            c = np.array([255, 61, 99]) / 255.0 #red\n            #box.render(ax[0], view=np.eye(4), colors=(c, c, c))\n            #print(x,y,z, name)\n    \n    \n    \n    \n    boxes = level5data.get_boxes(sample_cam_token)\n    box_list = []\n    #_, boxes, _ = level5data.get_sample_data(sample_lidar_token, box_vis_level=BoxVisibility.ANY, flat_vehicle_coordinates=True)\n    for box in boxes:\n        if box.name == 'animal':\n            ypr = Quaternion(pose_record[\"rotation\"]).yaw_pitch_roll\n            yaw = ypr[0]\n            box.translate(-np.array(pose_record[\"translation\"]))\n            box.rotate(Quaternion(scalar=np.cos(yaw / 2), vector=[0, 0, np.sin(yaw / 2)]).inverse)\n            box_list.append(box)\n    print(\"gth boxes: \")\n    for box in box_list:\n        x, y, z = box.center\n        w, l, h = box.wlh\n        name = box.name\n        print(x,y,z, name)\n        c = np.array(level5data.explorer.get_color(box.name)) / 255.0\n        box.render(ax[0], view=np.eye(4), colors=(c, c, c))\n    print(\"predicted from 2d boxes: \") \n    for box3d in pred_box3ds:\n        token0 = box3d.sample_token\n        if token0 == token:\n            x0, y0, z0 = box3d.translation\n            w, l, h = box3d.size\n            q0, q1, q2, q3 = box3d.rotation\n            cls_pred = box3d.name\n            cls_conf = box3d.score\n            q = Quaternion(w=q0, x=q1, y=q2, z=q3)\n            box1 = Box(\n                    center=[x0, y0, z0],\n                    size=[w, l, h],\n                    orientation=q,\n                    name=cls_pred,\n                    score=cls_conf,\n                    token=token\n                    )\n\n            box1.translate(-np.array(pose_record[\"translation\"]))\n            box1.rotate(Quaternion(pose_record[\"rotation\"]).inverse)\n            c = [0.5, 0, 1]\n            box1.render(ax[0], view=np.eye(4), colors=(c, c, c))\n            x, y, z = box1.center\n            x_min = x - l/2\n            y_min= y - w/2\n            rect = patches.Rectangle((x_min,y_min),l,w,linewidth=5,edgecolor='green',facecolor='none')\n            #ax[0].add_patch(rect)  \n            print(x, y, z)\n    x_lim = (x-2, x+2)\n    y_lim = (y-2, y+2)\n    z_lim = (-2, 2)\n    render_pointcloud_xyz(point_cloud, 2, ax[0], x_lim, y_lim, 1)\n    pca.fit(point_cloud[:,:2])\n    for length, vector in zip(pca.explained_variance_, pca.components_):\n        v = 5*vector * np.sqrt(length)\n        draw_vector(pca.mean_ + [1,1], pca.mean_+ [1,1] + v, ax[0])\n    vector = pca.components_[0]\n    yaw = np.degrees(np.arctan(vector[1]/vector[0]))\n    print(\"yaw angle: \", yaw)    \n    render_pointcloud_xz(point_cloud, 2, ax[1], x_lim, z_lim, 1)\n    render_pointcloud_yz(point_cloud, 2, ax[2], y_lim, z_lim, 1)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_box_list_3d_gth(boxes_list_3d, sample_cam_token):\n    sd_record = level5data.get(\"sample_data\", sample_cam_token)\n    cs_record = level5data.get(\"calibrated_sensor\", sd_record[\"calibrated_sensor_token\"])\n    sensor_record = level5data.get(\"sensor\", cs_record[\"sensor_token\"])\n    pose_record = level5data.get(\"ego_pose\", sd_record[\"ego_pose_token\"])\n    box_list_3d_gth = []\n\n    for j, pred_box in enumerate(boxes_list_3d):\n        x, y, z, w, l, h, q0, q1,q2,q3 = pred_box\n        #print(x, y, z)\n        x = float(x)\n        y = float(y)\n        z = float(z)\n        w = float(w)\n        h = float(h)\n        l = float(l)\n        q = Quaternion(w=q0, x=q1, y=q2, z=q3)\n        box1 = Box(\n                center=[x, y, z],\n                size=[w, l, h],\n                orientation=q,\n                name='animal',\n                score=1.0,\n                token=token\n                )\n        ypr = Quaternion(pose_record[\"rotation\"]).yaw_pitch_roll\n        yaw = ypr[0]\n        box1.translate(-np.array(pose_record[\"translation\"]))\n        box1.rotate(Quaternion(scalar=np.cos(yaw / 2), vector=[0, 0, np.sin(yaw / 2)]).inverse)\n        box_list_3d_gth.append(box1)\n    return box_list_3d_gth","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_box_list_3d_pred(pred_boxes_list_3d, sample_cam_token):\n    sd_record = level5data.get(\"sample_data\", sample_cam_token)\n    cs_record = level5data.get(\"calibrated_sensor\", sd_record[\"calibrated_sensor_token\"])\n    sensor_record = level5data.get(\"sensor\", cs_record[\"sensor_token\"])\n    pose_record = level5data.get(\"ego_pose\", sd_record[\"ego_pose_token\"])\n    box_list_3d_gth = []\n\n    for j, pred_box in enumerate(pred_boxes_list_3d):\n        cls_conf, x, y, z, w, l, h, yaw, cls_pred = pred_box\n        name = cls_pred\n        if name != 'animal':\n            cls_conf = float(cls_conf)\n            x = float(x)\n            y = float(y)\n            z = float(z)\n            w = float(w)\n            h = float(h)\n            l = float(l)\n            yaw = float(yaw)\n            #print(x, y, z, w, l, h, yaw)\n            q = euler2quat(0, 0, yaw)\n            q = Quaternion(w=q[0], x=q[1], y=q[2], z=q[3])\n            box1 = Box(\n                    center=[x, y, z],\n                    size=[w, l, h],\n                    orientation=q,\n                    name=cls_pred,\n                    score=cls_conf,\n                    token=token\n                    )\n            #print(box1)\n            ypr = Quaternion(pose_record[\"rotation\"]).yaw_pitch_roll\n            yaw = ypr[0]\n            box1.translate(-np.array(pose_record[\"translation\"]))\n            box1.rotate(Quaternion(scalar=np.cos(yaw / 2), vector=[0, 0, np.sin(yaw / 2)]).inverse)\n            #print(box1)\n            box_list_3d_gth.append(box1)\n    return box_list_3d_gth","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"token, cam_type, box_point_cloud = pred_box_point_cloud[13]     \nmy_sample = level5data.get('sample', token)\nsample_lidar_token = my_sample[\"data\"][\"LIDAR_TOP\"]\n\ndf = train_pred.loc[train_pred['Id'] == token]\npred_boxes_list_3d = np.array(df['PredictionString'])[0]\npred_boxes_list_3d = get_pred_boxes_list_3d(pred_boxes_list_3d)\nbox_list_3d_pred = get_box_list_3d_pred(pred_boxes_list_3d, sample_cam_token)\nshow_lidar_cloud(box_list_3d_pred, pred_box3ds, box_point_cloud, sample_lidar_token, token, cam_type)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"for idx in range(10):\n    print(\"Index: \", idx)\n    token = df_pred_2d.iloc[idx]['Id']\n\n    pred_boxes = df_pred_2d.iloc[idx]['2D']\n    pred_boxes_list = get_2d_boxes_list(pred_boxes)\n    for pred_box in pred_boxes_list:\n        coor = pred_box[:4]\n        x1 = int(float(coor[0]))\n        y1 = int(float(coor[1]))\n        x2 = int(float(coor[2]))\n        y2 = int(float(coor[3]))\n        cls_conf = pred_box[4]\n        cls_pred = pred_box[5]\n        cam_type = pred_box[6]\n        \n    my_sample = level5data.get('sample', token)\n    sample_cam_token = my_sample[\"data\"][cam_type]\n    sample_lidar_token = my_sample[\"data\"][\"LIDAR_TOP\"]\n\n    pcl_path, boxes, lidar_intrinsic = level5data.get_sample_data(sample_lidar_token)\n    #print(token, pcl_path)\n    pcd = LidarPointCloud.from_file(pcl_path)\n\n    df = train.loc[train['Id'] == token]\n    gth_boxes = np.array(df['3D'])[0]\n    gth_boxes_list_3d = get_gth_boxes_list_3d(gth_boxes)\n    box_list_3d_gth = get_box_list_3d_gth(gth_boxes_list_3d, sample_cam_token)\n    \n    df = train_pred.loc[train_pred['Id'] == token]\n    pred_boxes_list_3d = np.array(df['PredictionString'])[0]\n    pred_boxes_list_3d = get_pred_boxes_list_3d(pred_boxes_list_3d)\n    box_list_3d_pred = get_box_list_3d_pred(pred_boxes_list_3d, sample_cam_token)\n\n    pointsensor_token = sample_lidar_token\n    pointsensor = level5data.get(\"sample_data\", pointsensor_token)\n    cs_record = level5data.get(\"calibrated_sensor\", pointsensor[\"calibrated_sensor_token\"])\n    pcd.rotate(Quaternion(cs_record[\"rotation\"]).rotation_matrix)\n    pcd.translate(np.array(cs_record[\"translation\"]))\n    \n    poserecord = level5data.get(\"ego_pose\", pointsensor[\"ego_pose_token\"])\n    pcd.rotate(Quaternion(poserecord[\"rotation\"]).rotation_matrix)\n    pcd.translate(np.array(poserecord[\"translation\"]))\n    \n    cam = level5data.get(\"sample_data\", sample_cam_token)\n    # Third step: transform into the ego vehicle frame for the timestamp of the image.\n    poserecord = level5data.get(\"ego_pose\", cam[\"ego_pose_token\"])\n    pcd.translate(-np.array(poserecord[\"translation\"]))\n    pcd.rotate(Quaternion(poserecord[\"rotation\"]).rotation_matrix.T)\n    \n    point_cloud = pcd.points.transpose((1, 0))\n    #show_lidar_cloud(box_list_3d_pred, pred_box3ds, point_cloud, sample_lidar_token, token, cam_type)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"for idx in range(0,10):\n    token = df_pred_2d.iloc[idx]['Id']\n    pred_boxes = df_pred_2d.iloc[idx]['2D']\n    pred_boxes_list = get_2d_boxes_list(pred_boxes)\n    for pred_box in pred_boxes_list:\n        coor = pred_box[:4]\n        x1 = int(float(coor[0]))\n        y1 = int(float(coor[1]))\n        x2 = int(float(coor[2]))\n        y2 = int(float(coor[3]))\n        cls_conf = pred_box[4]\n        cls_pred = pred_box[5]\n        cam = pred_box[6]\n    my_sample = level5data.get('sample', token)\n    sample_cam_token = my_sample[\"data\"][cam]\n    sample_lidar_token = my_sample[\"data\"][\"LIDAR_TOP\"]\n    # Retrieve sensor & pose records\n    sd_record = level5data.get(\"sample_data\", sample_cam_token)\n    cs_record = level5data.get(\"calibrated_sensor\", sd_record[\"calibrated_sensor_token\"])\n    sensor_record = level5data.get(\"sensor\", cs_record[\"sensor_token\"])\n    pose_record = level5data.get(\"ego_pose\", sd_record[\"ego_pose_token\"])\n    # Get gth boxes\n    data_path, box_list_gth, cam_intrinsic = level5data.get_sample_data(sample_cam_token)\n    # Get predicted boxes\n    boxes = pred_box3ds\n    box_list_pred = []\n    for box in boxes:\n        token0 = box.sample_token\n        if token0 == token:\n            q = [0,0,0,1]\n            x,y,z = box.translation\n            w,l,h = box.size\n            cls_pred = box.name\n            score = box.score\n\n            #print(q,x,y,z,w,l,h)\n            box1 = Box(\n                center=[x, y, z],\n                size=[w, l, h],\n                orientation=Quaternion(q),\n                name=cls_pred,\n                score=score,\n                token=token\n            )\n\n            #print(box1)    \n            #  Move box to ego pose\n            box1.translate(-np.array(pose_record[\"translation\"]))\n            box1.rotate(Quaternion(pose_record[\"rotation\"]).inverse)\n            #  Move box to sensor coord system\n            box1.translate(-np.array(cs_record[\"translation\"]))\n            box1.rotate(Quaternion(cs_record[\"rotation\"]).inverse)\n            #print(box1)\n            box_list_pred.append(box1)\n\n    data = Image.open(data_path)\n    _, ax = plt.subplots(1, 2, figsize=(18, 9))\n    ax[0].imshow(data)\n\n    for box in box_list_gth:\n        if box.name == 'animal':\n            #print(box)\n            c = np.array(level5data.explorer.get_color(box.name)) / 255.0\n            box.render(ax[0], view=cam_intrinsic, normalize=True, colors=(c, c, c))\n\n    # Limit visible range.\n    ax[0].set_xlim(0, data.size[0])\n    ax[0].set_ylim(data.size[1], 0)\n    ax[0].set_title('Ground Truth Boxes')\n\n    ax[1].imshow(data)       \n    for box in box_list_pred:\n        c = np.array(level5data.explorer.get_color(box.name)) / 255.0\n        box.render(ax[1], view=cam_intrinsic, normalize=True, colors=(c, c, c))\n\n    # Limit visible range.\n    ax[1].set_xlim(0, data.size[0])\n    ax[1].set_ylim(data.size[1], 0)\n    ax[1].set_title('Predicted Boxes')","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"train_num = len(train)\nprint(train_num)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"from lyft_dataset_sdk.eval.detection.mAP_evaluation import  Box3D, recall_precision, get_class_names, get_average_precisions\nsample_tokens = []\n\nfor idx in range(train_num):\n    sample_tokens.append(train.iloc[idx]['Id'])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def load_groundtruth_boxes(nuscenes, sample_tokens):\n    gt_box3ds = []\n\n    # Load annotations and filter predictions and annotations.\n    for sample_token in tqdm(sample_tokens):\n\n        sample = nuscenes.get('sample', sample_token)\n        sample_annotation_tokens = sample['anns']\n\n        sample_lidar_token = sample[\"data\"][\"LIDAR_TOP\"]\n        lidar_data = level5data.get(\"sample_data\", sample_lidar_token)\n        ego_pose = level5data.get(\"ego_pose\", lidar_data[\"ego_pose_token\"])\n        ego_translation = np.array(ego_pose['translation'])\n        \n        for sample_annotation_token in sample_annotation_tokens:\n            sample_annotation = nuscenes.get('sample_annotation', sample_annotation_token)\n            sample_annotation_translation = sample_annotation['translation']\n            \n            class_name = sample_annotation['category_name']\n            if class_name ==\"animal\":\n                box3d = Box3D(\n                    sample_token=sample_token,\n                    translation=sample_annotation_translation,\n                    size=sample_annotation['size'],\n                    rotation=sample_annotation['rotation'],\n                    name=class_name\n                )\n                gt_box3ds.append(box3d)\n            \n    return gt_box3ds\n\ngt_box3ds = load_groundtruth_boxes(level5data, sample_tokens)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"len(gt_box3ds)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"import json\nARTIFACTS_FOLDER = \"./artifacts\"\nos.makedirs(ARTIFACTS_FOLDER, exist_ok=True)\ngt = [b.serialize() for b in gt_box3ds]\npr = [b.serialize() for b in pred_box3ds]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"gt[4], pr[0]","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"iou_th_range = np.linspace(0.5, 0.95, 10)\nmetric = {}\nprocesses = []\noutput_dir = 'tmp/'\noutput_dir = Path(output_dir)\noutput_dir.mkdir(parents=True, exist_ok=True)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"class_names = ['animal', 'bicycle', 'bus', 'car', 'emergency_vehicle',\n                    'motorcycle', 'other_vehicle', 'pedestrian', 'truck']","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def save_AP(gt, predictions, class_names, iou_threshold, output_dir):\n    #computes average precisions (AP) for a given threshold, and saves the metrics in a temp file \n    # use lyft's provided function to compute AP\n    AP = get_average_precisions(gt, predictions, class_names, iou_threshold)\n    # create a dict with keys as class names and values as their respective APs\n    metric = {c:AP[idx] for idx, c in enumerate(class_names)}\n\n    # save the dict in a temp file\n    summary_path = str(output_dir) + f'metric_summary_{iou_threshold}.json'\n    with open(str(summary_path), 'w') as f:\n        json.dump(metric, f)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"def get_metric_overall_AP(iou_th_range, output_dir, class_names):\n    ''' reads temp files and calculates overall per class APs.\n    returns:\n        `metric`: a dict with key as iou thresholds and value as dicts of class and their respective APs,\n        `overall_AP`: overall AP of each class\n    '''\n\n    metric = {}\n    overall_AP = np.zeros(len(class_names))\n    for iou_threshold in iou_th_range:\n        summary_path = str(output_dir) + f'metric_summary_{iou_threshold}.json'\n        with open(str(summary_path), 'r') as f:\n            data = json.load(f) # type(data): dict\n            metric[iou_threshold] = data\n            overall_AP += np.array([data[c] for c in class_names])\n    overall_AP /= len(iou_th_range)\n    return metric, overall_AP","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"time_start = time.time()\nfor iou_threshold in iou_th_range:\n    process = Process(target=save_AP, args=(gt, pr, class_names, iou_threshold, output_dir))\n    process.start()\n    processes.append(process)\n\nfor process in processes:\n    process.join()\nprint(\"Time to evaluate = \", time.time() - time_start)      ","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# get overall metrics\nmetric, overall_AP = get_metric_overall_AP(iou_th_range, output_dir, class_names)\nmetric['overall'] = {c: overall_AP[idx] for idx, c in enumerate(class_names)}\nmetric['mAP'] = np.mean(overall_AP)\nfor th in iou_th_range:\n    print(\"IOU threshold = \", th)\n    average_precisions = list(metric[th].values())\n    mAP = np.mean(average_precisions)\n    print(\"Average per class mean average precision = \", mAP)\n    for class_id in sorted(list(zip(class_names, average_precisions))):\n        print(class_id)\n    print(\"_______________________________________________\")\nprint(\"Overall mean average precision = \", metric['mAP'])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"time_start = time.time()\naverage_precisions = get_average_precisions(gt, pr, class_names, 0.01)\nmAP = np.mean(average_precisions)\nprint(\"Average per class mean average precision = \", mAP)\nfor class_id in sorted(list(zip(class_names, average_precisions.flatten().tolist()))):\n    print(class_id)\nprint(\"Time to evaluate = \", time.time() - time_start)  ","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"sub = {}\nfor i in tqdm(range(len(pred_box3ds))):\n    q0 = pred_box3ds[i].rotation[0]\n    q1 = pred_box3ds[i].rotation[1]\n    q2 = pred_box3ds[i].rotation[2]\n    q3 = pred_box3ds[i].rotation[3]\n    yaw = np.arctan2(2.0 * (q3 * q0 + q1 * q2) , - 1.0 + 2.0 * (q0 * q0 + q1 * q1))\n    pred =  str(pred_box3ds[i].score) + ' ' + str(pred_box3ds[i].center_x)  + ' '  + \\\n    str(pred_box3ds[i].center_y) + ' '  + str(pred_box3ds[i].center_z) + ' '  + \\\n    str(pred_box3ds[i].width) + ' ' \\\n    + str(pred_box3ds[i].length) + ' '  + str(pred_box3ds[i].height) + ' ' + str(yaw) + ' ' \\\n    + str(pred_box3ds[i].name) + ' ' \n        \n    if pred_box3ds[i].sample_token in sub.keys():     \n        sub[pred_box3ds[i].sample_token] += pred\n    else:\n        sub[pred_box3ds[i].sample_token] = pred        \n    \nsample_sub = pd.read_csv('../input/3d-object-detection-for-autonomous-vehicles/train.csv')\nfor token in set(sample_sub.Id.values).difference(sub.keys()):\n    sub[token] = ''","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"sub = pd.DataFrame(list(sub.items()))\nsub.columns = sample_sub.columns\nsub.head()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"sub.tail()","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"sub.to_csv('lyft3d_train_pred_animal.csv',index=False)","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}