{"cells":[{"metadata":{"trusted":true},"cell_type":"code","source":"!cp -r ../input/car-postion-prediction-utilsweights/* .","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"# Predicting Car Position and Yaw\n\nIn this kernel I will make use of the work made on my previous ones. I link them if you want further details.\n\n1.  https://www.kaggle.com/alvaroibrain/predicting-world-position-using-2d-features\n2. https://www.kaggle.com/alvaroibrain/car-yaw-prediction-from-images-baseline\n\nBasically, the approach is to use a 3-stage pipeline.\n\n 1. Run YOLO pretrained on COCO to detect the cars on the image and get bounding boxes.\n 2. Run the model generated in [1](https://www.kaggle.com/alvaroibrain/car-yaw-prediction-from-images-baseline/output) to get a estimation of the 3D world position of the cars.\n 3. Run the model generated in [2](https://www.kaggle.com/alvaroibrain/car-yaw-prediction-from-images-baseline/output) to predict the yaw of each car.\n\nHere, besides of the models, you will find some utility functions to work with the images.\n\nHope this will serve you as a baseline for your predictions or, at least, give you some ideas."},{"metadata":{"trusted":true},"cell_type":"code","source":"from matplotlib import pyplot as plt\nfrom PIL import Image\n\nimport numpy as np\nimport pandas as pd\nimport os\nimport cv2\nimport math\nimport tensorflow as tf\n\n# Our utility scripts to add abstraction to the models\nimport car_extractor\nimport yaw_pred\nimport camera_utils\nimport pos_pred","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"BASE_DIR_IMAGES = '/kaggle/input/pku-autonomous-driving/{}_images/'\nBASE_DIR_MASKS = '/kaggle/input/pku-autonomous-driving/{}_masks/'\nDIR_TRAIN_CSV = '/kaggle/input/pku-autonomous-driving/train.csv'","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"We load the three models with:"},{"metadata":{"trusted":true},"cell_type":"code","source":"yolo = car_extractor.get_model('yolo.h5')\nyaw_model = yaw_pred.get_model('yaw.h5')\nmodel_pos = pos_pred.load_model('pos_predictor.joblib')","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"Some utility functions to work with the data and display images"},{"metadata":{"trusted":true},"cell_type":"code","source":"def image_names(partition='train'):\n    return os.listdir(BASE_DIR_IMAGES.format(partition))\n\ndef load_img(img_name, partition='train'):\n    image = Image.open((BASE_DIR_IMAGES + '{}').format(partition, img_name))\n    image_mask = None\n    try:\n        image_mask = Image.open((BASE_DIR_MASKS + '{}').format(partition, img_name))\n    except FileNotFoundError:\n        pass\n    return image, image_mask\n\ndef get_pred_string(img_name):\n    name = img_name.split('.')[0]\n    coords_str = TRAIN_DF[TRAIN_DF['ImageId'] == name]['PredictionString'].iloc[0]\n    return coords_str\n    \ndef add_mask(img, mask):\n    mask_array = np.array(mask)\n    masked = np.array(img)\n    masked[mask_array > 0] = 255\n    return Image.fromarray(masked)\n\ndef parse_pred_string(string):\n    \"\"\" Converts an string to a list of arrays with the positions of the cars. \"\"\"\n    cars = []\n    split_string = string.split()\n    for i in range(0, len(split_string), 7):\n        arr_i = split_string[i:i+7]\n        arr_i = [float(i) for i in arr_i]\n        arr_i[0] = int(arr_i[0])\n        cars.append(arr_i)\n    return cars\n\ndef get_rot_matrix(euler_rot):\n    yaw, pitch, roll = euler_rot\n    \n    yaw, pitch, roll = -yaw, -pitch, -roll\n\n    # The data reference edges seem to be rotated. This matrices work.\n    # I got the idea of flipping thanks to: \n    # https://www.kaggle.com/zstusnoopy/visualize-the-location-and-3d-bounding-box-of-car#kln-87\n    \n    rot_x = np.array([\n                        [1,     0,              0         ],\n                        [0, math.cos(yaw), -math.sin(yaw) ],\n                        [0, math.sin(yaw), math.cos(yaw)  ]\n                    ])\n         \n    rot_y = np.array([\n                        [math.cos(pitch),  0,      math.sin(pitch) ],\n                        [0,                1,      0               ],\n                        [-math.sin(pitch), 0,      math.cos(pitch) ]\n                    ])\n                 \n    rot_z = np.array([\n                        [math.cos(roll), -math.sin(roll), 0],\n                        [math.sin(roll),  math.cos(roll), 0],\n                        [0,               0,              1]\n                    ])\n                     \n                     \n    rotation_matrix = np.dot(rot_x, np.dot(rot_y, rot_z))\n \n    return rotation_matrix\n\ndef get_point_arround(world_point, rotation_angles, offsets=[[0,0,2]]):\n    \"\"\"Adds points arround the center (world point) and rotates them to match the \n    car rotation (taking as origin world point).\n    This can be used to calculate several points arround a vehicle (draw 3D bounding boxes, etc). \n    \n    Params:\n    world_point: numpy array [3] (x,y,z) of the car in the world.\n    rotation_angles: numpy array [3]. (yaw, pitch, roll) in radians.\n    offsets: List[List[3]] Points arround the world point (by default it is only one point 2 units ahead of the vehicle center).\n    \n    Returns:\n        Numpy array with all the point(s) rotated acordingly to the center point.\n    \"\"\"\n    rot_from_origin = np.eye(4)\n    origin = world_point\n    rot_from_origin[:3, 3] = origin\n    rot_from_origin[:3, :3] = get_rot_matrix(rotation_angles)\n    rot_from_origin = rot_from_origin[:3, :]\n    \n    points = np.ones((len(offsets), 4))\n    points[:,:3] = np.array(offsets)\n    points = points.T\n            \n    point = np.dot(rot_from_origin, points).T\n    return point\n    \n\n    \ndef plot_car_directions(image, coords, as_matrix=False):\n    \"\"\" Plots a point on each car and a green arrow pointing towards its direction (yaw pitch roll) \n    \n    Parameters:\n    image: PIL Image\n    coords: Coordinate array of the cars (this is the parsed string from the dataset).\n    \"\"\"\n    im = np.array(image)\n    \n    world_coords = [x[-3:] for x in coords]\n    courses = [x[1:4] for x in coords]\n\n    transformed = camera_utils.to_cam_xy(world_coords)\n\n    points_directions = [get_point_arround(center_point, rotaton) for center_point, rotaton in zip(world_coords, courses)]\n    transformed_dests = np.array([camera_utils.to_cam_xy(points) for points in points_directions])\n    \n    # Car position (in 2D)\n    x0 = transformed[:,0]\n    y0 = transformed[:,1]\n\n    # Movement vector (yaw, pitch, roll) in 2D\n    x1 = transformed_dests[:,:,0].flatten()\n    y1 = transformed_dests[:,:,1].flatten()\n    \n    if as_matrix:\n        points = np.zeros((image.size[1], image.size[0], 3), dtype='uint8')\n        for i in range(len(world_coords)):\n            points[int(x0[i]),int(y0[i]), : ] = 255\n            points[int(x1[i]),int(y1[i]), : ] = 122\n            return points\n    \n    for i in range(len(world_coords)):\n        im = cv2.arrowedLine(im, (int(x0[i]),int(y0[i])), (int(x1[i]),int(y1[i])), (0,255,0), 3, tipLength=0.06)\n        im = cv2.circle(im, (int(x0[i]),int(y0[i])), 10, (255,0,0), -1)\n    return Image.fromarray(im)\n\ndef draw_car_boxes(image, car_boxes):\n    im_arr = np.array(image)\n    for i in range(len(car_boxes)):\n        point1 = (int(3384 * car_boxes[i][0]), int(2710 * car_boxes[i][1]))\n        point2 = (int(3384 * car_boxes[i][2]), int(2710 * car_boxes[i][3]))\n\n        cv2.rectangle(im_arr, point1, point2, (255,0,0), 5)\n\n    return Image.fromarray(im_arr)\n\nTRAIN_IMAGES = image_names('train')\nTRAIN_DF = pd.read_csv(DIR_TRAIN_CSV)\n\nTEST_IMAGES = image_names('test')","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"This function runs the pipeline of the three models on a given array of images"},{"metadata":{"trusted":true},"cell_type":"code","source":"def pipeline_pred(images):\n    \"\"\" Takes an array of images, runs the full pipeline (3 models) and returns a formated prediction array (as the requested one)\"\"\"\n    CAR_TYPE = -1\n    \n    world_coords = []\n    \n    boxes = car_extractor.find_cars(images, yolo)\n    \n    for boxi, imi in zip(boxes, images):\n        coords = [(camera_utils.IMAGE_WIDTH*((x[2]-x[0])/2 + x[0]), camera_utils.IMAGE_HEIGHT*((x[3]-x[1])/2 + x[1])) for x in boxi]\n        boxes_widths= [((x[2]-x[0])/2, (x[3]-x[1])/2 ) for x in boxi]\n        ray_angles = [camera_utils.get_ray_angle(c[0]) for c in coords]\n    \n        car_imgs = car_extractor.extract_car_images(imi, boxi)\n        car_imgs = [np.array(im) for im in car_imgs]\n    \n        yaws = yaw_pred.predict_yaw(car_imgs, ray_angles, yaw_model, return_confidences=True)\n        wp = pos_pred.predict(coords, boxes_widths, model_pos)\n    \n        pred_pos_string = [[CAR_TYPE, 0.15, y, 0, *w] for (y, w) in zip(yaws[0], wp)]\n        \n        world_coords.append(pred_pos_string)\n    \n    return world_coords","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### An example"},{"metadata":{"trusted":true},"cell_type":"code","source":"#%%timeit\ntest_image_name = TRAIN_IMAGES[1]\nimage, image_mask = load_img(test_image_name)\n\nres = pipeline_pred([image])\n\norig_coords_str = parse_pred_string(get_pred_string(test_image_name))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# Get and plot car boxes\nboxes = car_extractor.find_cars([image], yolo)\nim_boxes = draw_car_boxes(image, boxes[0])\n\nfig, ax = plt.subplots(figsize=(18,26))\nax.imshow(im_boxes)\nax.set_title('Detected cars')\nax.axis('off');","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"im_pred = plot_car_directions(image, res[0])\nim_actual = plot_car_directions(image, orig_coords_str)\n\n\nfig, ax = plt.subplots(1,2, figsize=(35,40))\nplt.subplots_adjust(wspace=0.01, hspace=0.01)\nax[0].imshow(im_pred)\nax[1].imshow(im_actual)\n\n\nax[0].set_title('Predicted')\nax[1].set_title('Real')\nax[0].axis('off')\nax[1].axis('off');","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"Let's test it on the test set"},{"metadata":{"trusted":true},"cell_type":"code","source":"image, image_mask = load_img(TEST_IMAGES[129], partition='test')\nres = pipeline_pred([image])","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"im_pred = plot_car_directions(image, res[0])\n\nfig, ax = plt.subplots(figsize=(18,26))\nax.imshow(im_pred)\nax.axis('off');","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Considerations\n- The 3D position estimation needs to be improved as sometimes, the deviations are noticeable\n- A function to generate a submission is needed."}],"metadata":{"kernelspec":{"display_name":"Python 3","language":"python","name":"python3"},"language_info":{"codemirror_mode":{"name":"ipython","version":3},"file_extension":".py","mimetype":"text/x-python","name":"python","nbconvert_exporter":"python","pygments_lexer":"ipython3","version":"3.7.3"}},"nbformat":4,"nbformat_minor":1}