{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# Environment Perception For Self-Driving Cars\n",
    "\n",
    "Welcome to the final assignment for this course. In this assignment, you will learn how to use the material so far to extract useful scene information to allow self-driving cars to safely and reliably traverse their environment.\n",
    "\n",
    "\n",
    "**In this assignment, you will:**\n",
    "- Use the output of semantic segmentation neural networks to implement drivable space estimation in 3D.\n",
    "- Use the output of semantic segmentation neural networks to implement lane estimation.\n",
    "- Use the output of semantic segmentation to filter errors in the output of 2D object detectors. \n",
    "- Use the filtered 2D object detection results to determine how far obstacles are from the self-driving car.\n",
    "\n",
    "For most exercises, you are provided with a suggested outline. You are encouraged to diverge from the outline if you think there is a better, more efficient way to solve a problem.\n",
    "\n",
    "Please go through cells in order. Lower cells will depend on higher cells to work properly.\n",
    "\n",
    "You are only allowed to use the packages loaded bellow, mainly numpy, OpenCV, and the custom functions explained in the notebook. Run the cell bellow to import the required packages:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "from matplotlib import pyplot as plt\n",
    "from m6bk import *\n",
    "\n",
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "\n",
    "np.random.seed(1)\n",
    "np.set_printoptions(precision=2, threshold=np.nan)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 0 - Loading and Visualizing the Data\n",
    "We provide you with a convenient dataset handler class to read and iterate through samples taken from the CARLA simulator. Run the following code to create a dataset handler object. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "dataset_handler = DatasetHandler()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The dataset handler contains three test data frames 0, 1, and 2. Each frames contains:\n",
    "- DatasetHandler().rgb: a camera RGB image \n",
    "- DatasetHandler().depth: a depth image containing the depth in meters for every pixel.\n",
    "- DatasetHandler().segmentation: an image containing the output of a semantic segmentation neural network as the category per pixel. \n",
    "- DatasetHandler().object_detection: a numpy array containing the output of an object detection network. \n",
    "\n",
    "Frame 0 will be used throughout this note book as a running example. **Frame 1 will be used for submission and grading of this assesment.** Frame 2 is provided as a challange for learners interested in a more difficult example.\n",
    "\n",
    "The current data frame being read can be known through the following line of code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "dataset_handler.current_frame"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    " Upon creation of the dataset handler object, frame 0 will be automatically read and loaded. The frame contents can be accessed by using four different attributes of the dataset handler object: image, depth, object_detection, and semantic segmentation. As an example, to access the image, camera calibration matrix, and depth run the following three cells:    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "image = dataset_handler.image\n",
    "plt.imshow(image)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "k = dataset_handler.k\n",
    "print(k)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "depth = dataset_handler.depth\n",
    "plt.imshow(depth, cmap='jet')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The semantic segmentation output can be accessed in a similar manner through:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "segmentation = dataset_handler.segmentation\n",
    "plt.imshow(segmentation)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Segmentation Category Mappings:\n",
    "The output segmentation image contains mapping indices from every pixel to a road scene category. To visualize the semantic segmentation output, we map the mapping indices to different colors. The mapping indices and visualization colors for every road scene category can be found in the following table: \n",
    "\n",
    "|Category |Mapping Index| Visualization Color|\n",
    "| --- | --- | --- |\n",
    "| Background | 0 | Black |\n",
    "| Buildings | 1 | Red |\n",
    "| Pedestrians | 4 | Teal |\n",
    "| Poles | 5 | White |\n",
    "| Lane Markings | 6| Purple |\n",
    "| Roads | 7 | Blue |\n",
    "| Side Walks| 8 | Yellow |\n",
    "| Vehicles| 10 | Green |\n",
    "\n",
    "The vis_segmentation function of the dataset handler transforms the index image to a color image for visualization: "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "colored_segmentation = dataset_handler.vis_segmentation(segmentation)\n",
    "\n",
    "plt.imshow(colored_segmentation)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The set_frame function takes as an input a frame number from 0 to 2 and loads that frame allong with all its associated data. It will be useful for testing and submission at the end of this assesment."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "dataset_handler.set_frame(2)\n",
    "dataset_handler.current_frame"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "image = dataset_handler.image\n",
    "plt.imshow(image)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1 - Drivable Space Estimation Using Semantic Segmentation Output\n",
    "\n",
    "Your first task is to implement drivable space estimation in 3D. You are given the output of a semantic segmentation neural network, the camera calibration matrix K, as well as the depth per pixel.\n",
    "\n",
    "### 1.1 - Estimating the x, y, and z coordinates of every pixel in the image:\n",
    "\n",
    "You will be using the equations learned in module 1 to compute the x, y, and z coordinates of every pixel in the image. As a reminder, the equations to get the required 3D coordinates are:\n",
    "\n",
    "$$z = depth $$\n",
    "\n",
    "$x = \\frac{(u - c_u) * z}{f} \\tag{1}$\n",
    "\n",
    "$y = \\frac{(v - c_v) * z}{f} \\tag{2}$\n",
    "\n",
    "Here, $c_u$, $c_v$, and $f$ are the intrinsic calibration parameters found in the camera calibration matrix K such that:\n",
    "\n",
    "$$K = \\begin{pmatrix} f & 0 & u_c \\\\ 0 & f & u_v \\\\ 0& 0 & 1 \\end{pmatrix}$$\n",
    "\n",
    "***Note***: Make sure you are on frame 0 for the rest of this assessment. You will use the rest of the frames for testing after the assessment is done.\n",
    "\n",
    "**Exercise**: Implement the estimation of the x and y coordinates of every pixel using equations (1) and (2):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# GRADED FUNCTION: xy_from_depth\n",
    "def xy_from_depth(depth, k):\n",
    "    \"\"\"\n",
    "    Computes the x, and y coordinates of every pixel in the image using the depth map and the calibration matrix.\n",
    "\n",
    "    Arguments:\n",
    "    depth -- tensor of dimension (H, W), contains a depth value (in meters) for every pixel in the image.\n",
    "    k -- tensor of dimension (3x3), the intrinsic camera matrix\n",
    "\n",
    "    Returns:\n",
    "    x -- tensor of dimension (H, W) containing the x coordinates of every pixel in the camera coordinate frame.\n",
    "    y -- tensor of dimension (H, W) containing the y coordinates of every pixel in the camera coordinate frame.\n",
    "    \"\"\"\n",
    "    ### START CODE HERE ### (≈ 7 lines in total)\n",
    "\n",
    "    # Get the shape of the depth tensor\n",
    "\n",
    "    # Grab required parameters from the K matrix\n",
    "\n",
    "    # Generate a grid of coordinates corresponding to the shape of the depth map\n",
    "\n",
    "    # Compute x and y coordinates\n",
    "\n",
    "    ### END CODE HERE ###\n",
    "    \n",
    "    return x, y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "dataset_handler.set_frame(0)\n",
    "\n",
    "k = dataset_handler.k\n",
    "\n",
    "z = dataset_handler.depth\n",
    "\n",
    "x, y = xy_from_depth(z, k)\n",
    "\n",
    "print('x[800,800] = ' + str(x[800, 800]))\n",
    "print('y[800,800] = ' + str(y[800, 800]))\n",
    "print('z[800,800] = ' + str(z[800, 800]) + '\\n')\n",
    "\n",
    "print('x[500,500] = ' + str(x[500, 500]))\n",
    "print('y[500,500] = ' + str(y[500, 500]))\n",
    "print('z[500,500] = ' + str(z[500, 500]) + '\\n')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Expected Output**:\n",
    "\n",
    "$x[800,800] = 0.720$\n",
    "<br />\n",
    "$y[800,800] = 1.436$\n",
    "<br />\n",
    "$z[800,800] = 2.864$\n",
    "\n",
    "$x[500,500] = -9.5742765625$\n",
    "<br />\n",
    "$y[500,500] = 1.4464734375$\n",
    "<br />\n",
    "$z[500,500] = 44.083$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1.2 - Estimating The Ground Plane Using RANSAC:\n",
    "\n",
    "In the context of self-driving cars, drivable space includes any space that the car is physically capable of traversing in 3D. The task of estimating the drivable space is equivalent to estimating pixels belonging to the ground plane in the scene. For the next exercise, you will use RANSAC to estimate the ground plane in the 3D camera coordinate frame from the x,y, and z coordinates estimated above. \n",
    "\n",
    "The first step is to process the semantic segmentation output to extract the relevant pixels belonging to the class you want consider as ground. For this assessment, that class is the road class with a mapping index of 7. To extract the x,y,z coordinates of the road, run the following cell:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Get road mask by choosing pixels in segmentation output with value 7\n",
    "road_mask = np.zeros(segmentation.shape)\n",
    "road_mask[segmentation == 7] = 1\n",
    "\n",
    "# Show road mask\n",
    "plt.imshow(road_mask)\n",
    "\n",
    "# Get x,y, and z coordinates of pixels in road mask\n",
    "x_ground = x[road_mask == 1]\n",
    "y_ground = y[road_mask == 1]\n",
    "z_ground = dataset_handler.depth[road_mask == 1]\n",
    "xyz_ground = np.stack((x_ground, y_ground, z_ground))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next step is to use the extracted x, y, and z coordinates of pixels belonging to the road to estimate the ground plane. RANSAC will be used for robustness against outliers."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Exercise**: Implement RANSAC for plane estimation. Here are the 6 steps:\n",
    "1. Choose a minimum of 3 points from xyz_ground at random.\n",
    "2. Compute the ground plane model using the chosen random points, and the provided function compute_plane.\n",
    "3. Compute the distance from the ground plane model to every point in xyz_ground, and compute the number of inliers based on a distance threshold.\n",
    "4. Check if the current number of inliers is greater than all previous iterations and keep the inlier set with the largest number of points.  \n",
    "5. Repeat until number of iterations $\\geq$ a preset number of iterations, or number of inliers $\\geq$ minimum number of inliers.\n",
    "6. Recompute and return a plane model using all inliers in the final inlier set. \n",
    "\n",
    "Useful functions: `np.random.choice()`, `compute_plane()`, `dist_to_plane()`.\n",
    "\n",
    "\n",
    "The two custom functions are provided to help you finish this part:\n",
    "\n",
    "1. ***compute_plane(xyz):***\n",
    "```\n",
    "    Computes plane coefficients a,b,c,d of the plane in the form ax+by+cz+d = 0\n",
    "\n",
    "    Arguments:\n",
    "    xyz -- tensor of dimension (3, N), contains points needed to fit plane.\n",
    "    k -- tensor of dimension (3x3), the intrinsic camera matrix\n",
    "\n",
    "    Returns:\n",
    "    p -- tensor of dimension (1, 4) containing the plane parameters a,b,c,d\n",
    "```\n",
    "\n",
    "2. ***dist_to_plane(plane, x, y, z):***\n",
    "\n",
    "```\n",
    "    Computes distance from N points to a plane in 3D, given the plane parameters and the x,y,z coordinate of points.\n",
    "\n",
    "    Arguments:\n",
    "    plane -- tensor of dimension (4,1), containing the plane parameters [a,b,c,d]\n",
    "    x -- tensor of dimension (Nx1), containing the x coordinates of the points\n",
    "    y -- tensor of dimension (Nx1), containing the y coordinates of the points\n",
    "    z -- tensor of dimension (Nx1), containing the z coordinates of the points\n",
    "\n",
    "\n",
    "    Returns:\n",
    "    distance -- tensor of dimension (N, 1) containing the distance between points and the plane\n",
    "```\n",
    "\n",
    "The functions are already loaded through the import statement at the beginning of the notebook. You also could perform plane estimation yourself if you are up for a challenge!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# GRADED FUNCTION: RANSAC Plane Fitting\n",
    "\n",
    "def ransac_plane_fit(xyz_data):\n",
    "    \"\"\"\n",
    "    Computes plane coefficients a,b,c,d of the plane in the form ax+by+cz+d = 0\n",
    "    using ransac for outlier rejection.\n",
    "\n",
    "    Arguments:\n",
    "    xyz_data -- tensor of dimension (3, N), contains all data points from which random sampling will proceed.\n",
    "    num_itr -- \n",
    "    distance_threshold -- Distance threshold from plane for a point to be considered an inlier.\n",
    "\n",
    "    Returns:\n",
    "    p -- tensor of dimension (1, 4) containing the plane parameters a,b,c,d\n",
    "    \"\"\"\n",
    "    \n",
    "    ### START CODE HERE ### (≈ 23 lines in total)\n",
    "    \n",
    "    # Set thresholds:\n",
    "    num_itr = None  # RANSAC maximum number of iterations\n",
    "    min_num_inliers = None  # RANSAC minimum number of inliers\n",
    "    distance_threshold = None  # Maximum distance from point to plane for point to be considered inlier\n",
    "    \n",
    "    for i in range(num_itr):\n",
    "        # Step 1: Choose a minimum of 3 points from xyz_data at random.\n",
    "        \n",
    "        # Step 2: Compute plane model\n",
    "        \n",
    "        # Step 3: Find number of inliers\n",
    "        \n",
    "        # Step 4: Check if the current number of inliers is greater than all previous iterations and keep the inlier set with the largest number of points.\n",
    "\n",
    "        # Step 5: Check if stopping criterion is satisfied and break.         \n",
    "\n",
    "        \n",
    "    # Step 6: Recompute the model parameters using largest inlier set.         \n",
    "\n",
    "    ### END CODE HERE ###\n",
    "    \n",
    "    return output_plane "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "p_final = ransac_plane_fit(xyz_ground)\n",
    "print('Ground Plane: ' + str(p_final))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Expected Output**:\n",
    "\n",
    "Ground Plane: [0.01791606 -0.99981332  0.00723433  1.40281479]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To verify that the estimated plane is correct, we can visualize the inlier set computed on the whole image. Use the cell bellow to compute and visualize the ground mask in 2D image space."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "dist = np.abs(dist_to_plane(p_final, x, y, z))\n",
    "\n",
    "ground_mask = np.zeros(dist.shape)\n",
    "\n",
    "ground_mask[dist < 0.1] = 1\n",
    "ground_mask[dist > 0.1] = 0\n",
    "\n",
    "plt.imshow(ground_mask)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We also provide a function to visualize the estimated drivable space in 3D. Run the following cell to visualize your estimated drivable space in 3D."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "dataset_handler.plot_free_space(ground_mask)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The above visualization only shows where the self-driving car can physically travel. The obstacles such as the SUV to the left of the image, can be seen as dark pixels in our visualization: "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<tr>\n",
    "<td> <img src=\"images/image.png\" style=\"width:320px;height:240px;\">   </td>\n",
    "<td> <img src=\"images/occ_grid.png\" style=\"width:240px;height:240px;\">   </td>\n",
    "</tr>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "However, estimating the drivable space is not enough for the self-driving car to get on roads. The self-driving car still needs to perform lane estimation to know where it is legally allowed to drive. Once you are comfortable with the estimated drivable space, continue the assessment to estimate the lane where the car can drive.\n",
    "\n",
    "## 2 - Lane Estimation Using The Semantic Segmentation Output\n",
    "\n",
    "Your second task for this assessment is to use the output of semantic segmentation to estimate the lane boundaries of the current lane the self-driving car is using. This task can be separated to two subtasks, lane line estimation, and post-processing through horizontal line filtering and similar line merging.\n",
    "\n",
    "### 2.1 Estimating Lane Boundary Proposals:\n",
    "The first step to perform this task is to estimate any line that qualifies as a lane boundary using the output from semantic segmentation. We call these lines 'proposals'.\n",
    "\n",
    "**Exercise**: Estimate lane line proposals using OpenCv functions. Here are the 3 steps:\n",
    "1. Create an image containing the semantic segmentation pixels belonging to categories relevant to the lane boundaries, similar to what we have done previously for the road plane. For this assessment, these pixels have the value of 6 and 8 in the neural network segmentation output.\n",
    "2. Perform edge detection on the derived lane boundary image.\n",
    "3. Perform line estimation on the output of edge detection.\n",
    "\n",
    "Useful functions: `cv2.Canny()`, `cv2.HoughLinesP()`, `np.squeeze()`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# GRADED FUNCTION: estimate_lane_lines\n",
    "def estimate_lane_lines(segmentation_output):\n",
    "    \"\"\"\n",
    "    Estimates lines belonging to lane boundaries. Multiple lines could correspond to a single lane.\n",
    "\n",
    "    Arguments:\n",
    "    segmentation_output -- tensor of dimension (H,W), containing semantic segmentation neural network output\n",
    "    minLineLength -- Scalar, the minimum line length\n",
    "    maxLineGap -- Scalar, dimension (Nx1), containing the z coordinates of the points\n",
    "\n",
    "    Returns:\n",
    "    lines -- tensor of dimension (N, 4) containing lines in the form of [x_1, y_1, x_2, y_2], where [x_1,y_1] and [x_2,y_2] are\n",
    "    the coordinates of two points on the line in the (u,v) image coordinate frame.\n",
    "    \"\"\"\n",
    "    ### START CODE HERE ### (≈ 7 lines in total)\n",
    "    # Step 1: Create an image with pixels belonging to lane boundary categories from the output of semantic segmentation\n",
    "\n",
    "\n",
    "    # Step 2: Perform Edge Detection using cv2.Canny()\n",
    "\n",
    "\n",
    "    # Step 3: Perform Line estimation using cv2.HoughLinesP()\n",
    "\n",
    "    \n",
    "    # Note: Make sure dimensions of returned lines is (N x 4)\n",
    "    ### END CODE HERE ###\n",
    "\n",
    "    return lines"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "lane_lines = estimate_lane_lines(segmentation)\n",
    "\n",
    "print(lane_lines.shape)\n",
    "\n",
    "plt.imshow(dataset_handler.vis_lanes(lane_lines))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "***Expected Output***\n",
    "\n",
    "<img src=\"images/lanes_1.png\" style=\"width:320px;height:240px;\"> "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2.2 - Merging and Filtering Lane Lines:\n",
    "\n",
    "The second subtask to perform the estimation of the current lane boundary is to merge redundant lines, and filter out any horizontal lines apparent in the image. Merging redundant lines can be solved through grouping lines with similar slope and intercept. Horizontal lines can be filtered out through slope thresholding. \n",
    "\n",
    "**Exercise**: Post-process the output of the function ``estimate_lane_lines`` to merge similar lines, and filter out horizontal lines using the slope and the intercept. The three steps are:\n",
    "1. Get every line's slope and intercept using the function provided.\n",
    "2. Determine lines with slope less than horizontal slope threshold. Filtering can be performed later if needed.\n",
    "3. Cluster lines based on slope and intercept as you learned in Module 6 of the course. \n",
    "4. Merge all lines in clusters using mean averaging.\n",
    "\n",
    "Usefull Functions:\n",
    "1. ***get_slope_intecept(lines):***\n",
    "\n",
    "```\n",
    "    Computes distance from N points to a plane in 3D, given the plane parameters and the x,y,z coordinate of points.\n",
    "\n",
    "    Arguments:\n",
    "    lines -- tensor of dimension (N,4) containing lines in the form of [x_1, y_1, x_2, y_2], the coordinates of two points on       the line\n",
    "\n",
    "    Returns:\n",
    "    slopes -- tensor of dimension (N, 1) containing the slopes of the lines\n",
    "    intercepts -- tensor of dimension (N,1) containing the intercepts of the lines\n",
    "```\n",
    "\n",
    "This function is already loaded through the import statement at the beginning of the notebook. You also could perform plane estimation yourself if you are up for a challenge!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Graded Function: merge_lane_lines\n",
    "def merge_lane_lines(\n",
    "        lines):\n",
    "    \"\"\"\n",
    "    Merges lane lines to output a single line per lane, using the slope and intercept as similarity measures.\n",
    "    Also, filters horizontal lane lines based on a minimum slope threshold.\n",
    "\n",
    "    Arguments:\n",
    "    lines -- tensor of dimension (N, 4) containing lines in the form of [x_1, y_1, x_2, y_2],\n",
    "    the coordinates of two points on the line.\n",
    "\n",
    "    Returns:\n",
    "    merged_lines -- tensor of dimension (N, 4) containing lines in the form of [x_1, y_1, x_2, y_2],\n",
    "    the coordinates of two points on the line.\n",
    "    \"\"\"\n",
    "    \n",
    "    ### START CODE HERE ### (≈ 25 lines in total)\n",
    "    \n",
    "    # Step 0: Define thresholds\n",
    "    slope_similarity_threshold = 0.1\n",
    "    intercept_similarity_threshold = 40\n",
    "    min_slope_threshold = 0.3\n",
    "    \n",
    "    # Step 1: Get slope and intercept of lines\n",
    "    slopes, intercepts = get_slope_intecept(lines)\n",
    "    \n",
    "    # Step 2: Determine lines with slope less than horizontal slope threshold.\n",
    "\n",
    "    # Step 3: Iterate over all remaining slopes and intercepts and cluster lines that are close to each other using a slope and intercept threshold.\n",
    "    for slope, intercept in zip(slopes, intercepts):\n",
    "        \n",
    "\n",
    "    # Step 4: Merge all lines in clusters using mean averaging\n",
    "\n",
    "    \n",
    "    # Note: Make sure dimensions of returned lines is (N x 4)\n",
    "    ### END CODE HERE ###\n",
    "    return merged_lines"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "merged_lane_lines = merge_lane_lines(lane_lines)\n",
    "\n",
    "plt.imshow(dataset_handler.vis_lanes(merged_lane_lines))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "***Expected Output***\n",
    "\n",
    "<img src=\"images/lanes_2.png\" style=\"width:320px;height:240px;\"> "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You now should have one line per lane as an output! The final step is to extrapolate the lanes to start at the beginning of the road, and end at the end of the road, and to determine the lane markings belonging to the current lane. We provide you with functions that perform these tasks in the cell bellow. Run the cell to visualize the final lane boundaries!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "max_y = dataset_handler.image.shape[0]\n",
    "min_y = np.min(np.argwhere(road_mask == 1)[:, 0])\n",
    "\n",
    "extrapolated_lanes = extrapolate_lines(merged_lane_lines, max_y, min_y)\n",
    "final_lanes = find_closest_lines(extrapolated_lanes, dataset_handler.lane_midpoint)\n",
    "plt.imshow(dataset_handler.vis_lanes(final_lanes))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "***Expected Output***\n",
    "\n",
    "<img src=\"images/lanes_final.png\" style=\"width:320px;height:240px;\"> "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3 - Computing Minimum Distance To Impact Using The Output of 2D Object Detection.\n",
    "\n",
    "Your final task for this assessment is to use 2D object detection output to determine the minimum distance to impact with objects in the scene. However, the task is complicated by the fact that the provided 2D detections are from a high recall, low precision 2D object detector. You will first be using the semantic segmentation output to determine which bounding boxes are valid. Then, you will compute the minimum distance to impact using the remaining bounding boxes and the depth image. Let us begin with a visualization of the output detection for our current frame. For visualization, you use the provided dataset handler function ``vis_object_detection`` as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "detections = dataset_handler.object_detection\n",
    "\n",
    "plt.imshow(dataset_handler.vis_object_detection(detections))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Detections have the format [category, x_min, y_min, x_max, y_max, score]. The Category is a string signifying the classification of the bounding box such as 'Car', 'Pedestrian' or 'Cyclist'. [x_min,y_min] are the coordinates of the top left corner, and [x_max,y_max] are the coordinates of the bottom right corners of the objects. The score signifies the output of the softmax from the neural network."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print(detections)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.1 - Filtering Out Unreliable Detections:\n",
    "The first thing you can notice is that an wrong detection occures on the right side of the image. What is interestingis that this wrong detection has a high output score of 0.76 for being a car. Furthermore, two bounding boxes are assigned to the vehicle to the left of the image, both with a very high score, greater than 0.9. This behaviour is expected from a high precision, low recall object detector. To solve this problem, the output of the semantic segmentation network has to be used to eliminate unreliable detections.\n",
    "\n",
    "**Exercise**: Eliminate unreliable detections using the output of semantic segmentation. The three steps are:\n",
    "1. For each detection, compute how many pixels in the bounding box belong to the category predicted by the neural network.\n",
    "2. Devide the computed number of pixels by the area of the bounding box (total number of pixels).\n",
    "3. If the ratio is greater than a threshold keep the detection. Else, remove the detection from the list of detections.\n",
    "\n",
    "Usefull functions: ``np.asfarray()``\n",
    "\n",
    "***Note***: Make sure to handle both the 'Car' and 'Pedestrian' categories in the code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Graded Function: filter_detections_by_segmentation\n",
    "def filter_detections_by_segmentation(detections, segmentation_output):\n",
    "    \"\"\"\n",
    "    Filter 2D detection output based on a semantic segmentation map.\n",
    "\n",
    "    Arguments:\n",
    "    detections -- tensor of dimension (N, 5) containing detections in the form of [Class, x_min, y_min, x_max, y_max, score].\n",
    "    \n",
    "    segmentation_output -- tensor of dimension (HxW) containing pixel category labels.\n",
    "    \n",
    "    Returns:\n",
    "    filtered_detections -- tensor of dimension (N, 5) containing detections in the form of [Class, x_min, y_min, x_max, y_max, score].\n",
    "\n",
    "    \"\"\"\n",
    "    ### START CODE HERE ### (≈ 20 lines in total)\n",
    "    \n",
    "    # Set ratio threshold:\n",
    "    ratio_threshold = 0.3  # If 1/3 of the total pixels belong to the target category, the detection is correct.\n",
    "    \n",
    "    for detection in detections:\n",
    "        \n",
    "        # Step 1: Compute number of pixels belonging to the category for every detection.\n",
    "        \n",
    "        # Step 2: Devide the computed number of pixels by the area of the bounding box (total number of pixels).\n",
    "            \n",
    "        # Step 3: If the ratio is greater than a threshold keep the detection. Else, remove the detection from the list of detections.\n",
    "\n",
    " \n",
    "    ### END CODE HERE ###\n",
    "    \n",
    "    return filtered_detections"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "filtered_detections = filter_detections_by_segmentation(detections, segmentation)\n",
    "\n",
    "plt.imshow(dataset_handler.vis_object_detection(filtered_detections))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.2 - Estimating Minimum Distance To Impact:\n",
    "\n",
    "The final task for this assessment is to estimate the minimum distance to every bounding box in the input detections. This can be performed by simply taking the minimum distance from the pixels in the bounding box to the camera center.\n",
    "\n",
    "**Exercise**: Compute the minimum distance to impact between every object remaining after filtering and the self-driving car. The two steps are:\n",
    "\n",
    "1. Compute the distance to the camera center using the x,y,z arrays from  part I. This can be done according to the equation: $ distance = \\sqrt{x^2 + y^2 + z^2}$.\n",
    "2. Find the value of the minimum distance of all pixels inside the bounding box."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Graded Function: find_min_distance_to_detection:\n",
    "def find_min_distance_to_detection(detections, x, y, z):\n",
    "    \"\"\"\n",
    "    Filter 2D detection output based on a semantic segmentation map.\n",
    "\n",
    "    Arguments:\n",
    "    detections -- tensor of dimension (N, 5) containing detections in the form of [Class, x_min, y_min, x_max, y_max, score].\n",
    "    \n",
    "    x -- tensor of dimension (H, W) containing the x coordinates of every pixel in the camera coordinate frame.\n",
    "    y -- tensor of dimension (H, W) containing the y coordinates of every pixel in the camera coordinate frame.\n",
    "    z -- tensor of dimensions (H,W) containing the z coordinates of every pixel in the camera coordinate frame.\n",
    "    Returns:\n",
    "    min_distances -- tensor of dimension (N, 1) containing distance to impact with every object in the scene.\n",
    "\n",
    "    \"\"\"\n",
    "    ### START CODE HERE ### (≈ 20 lines in total)\n",
    "\n",
    "    for detection in detections:\n",
    "        # Step 1: Compute distance of every pixel in the detection bounds\n",
    "        \n",
    "        # Step 2: Find minimum distance\n",
    "\n",
    "    ### END CODE HERE ###\n",
    "    return min_distances"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "min_distances = find_min_distance_to_detection(filtered_detections, x, y, z)\n",
    "\n",
    "print('Minimum distance to impact is: ' + str(min_distances))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Expected Output**\n",
    "\n",
    "Minimum distance to impact is: 8.51\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Run the cell bellow to visualize your estimated distance along with the 2D detection output."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "font = {'family': 'serif','color': 'red','weight': 'normal','size': 12}\n",
    "\n",
    "im_out = dataset_handler.vis_object_detection(filtered_detections)\n",
    "\n",
    "for detection, min_distance in zip(filtered_detections, min_distances):\n",
    "    bounding_box = np.asfarray(detection[1:5])\n",
    "    plt.text(bounding_box[0], bounding_box[1] - 20, 'Distance to Impact:' + str(np.round(min_distance, 2)) + ' m', fontdict=font)\n",
    "\n",
    "plt.imshow(im_out)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4 - Submission:\n",
    "\n",
    "Evaluation of all the functions will be based on **three** outputs for frame 1 of the dataset:\n",
    "1. The estimated ground plane from part 1.\n",
    "2. The estimated lanes from part 2.\n",
    "3. The estimated distances from part 3. \n",
    "\n",
    "Please run the cell bellow, then copy its output to the provided output.yaml file for submission on the programming assignment page.   "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "dataset_handler = DatasetHandler()\n",
    "dataset_handler.set_frame(1)\n",
    "segmentation = dataset_handler.segmentation\n",
    "detections = dataset_handler.object_detection\n",
    "z = dataset_handler.depth\n",
    "\n",
    "# Part 1\n",
    "k = dataset_handler.k\n",
    "x, y = xy_from_depth(z, k)\n",
    "road_mask = np.zeros(segmentation.shape)\n",
    "road_mask[segmentation == 7] = 1\n",
    "x_ground = x[road_mask == 1]\n",
    "y_ground = y[road_mask == 1]\n",
    "z_ground = dataset_handler.depth[road_mask == 1]\n",
    "xyz_ground = np.stack((x_ground, y_ground, z_ground))\n",
    "p_final = ransac_plane_fit(xyz_ground)\n",
    "\n",
    "# Part II\n",
    "lane_lines = estimate_lane_lines(segmentation)\n",
    "merged_lane_lines = merge_lane_lines(lane_lines)\n",
    "max_y = dataset_handler.image.shape[0]\n",
    "min_y = np.min(np.argwhere(road_mask == 1)[:, 0])\n",
    "\n",
    "extrapolated_lanes = extrapolate_lines(merged_lane_lines, max_y, min_y)\n",
    "final_lanes = find_closest_lines(extrapolated_lanes, dataset_handler.lane_midpoint)\n",
    "\n",
    "# Part III\n",
    "filtered_detections = filter_detections_by_segmentation(detections, segmentation)\n",
    "min_distances = find_min_distance_to_detection(filtered_detections, x, y, z)\n",
    "\n",
    "# Print Submission Info\n",
    "\n",
    "final_lane_printed = [list(np.round(lane)) for lane in final_lanes]\n",
    "print('plane:') \n",
    "print(list(np.round(p_final, 2)))\n",
    "print('\\n lanes:')\n",
    "print(final_lane_printed)\n",
    "print('\\n min_distance')\n",
    "print(list(np.round(min_distances, 2)))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Visualize your Results:\n",
    "\n",
    "Make sure your results visualization is appealing before submitting your results."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Original Image\n",
    "plt.imshow(dataset_handler.image)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Part I\n",
    "dist = np.abs(dist_to_plane(p_final, x, y, z))\n",
    "\n",
    "ground_mask = np.zeros(dist.shape)\n",
    "\n",
    "ground_mask[dist < 0.1] = 1\n",
    "ground_mask[dist > 0.1] = 0\n",
    "\n",
    "plt.imshow(ground_mask)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Part II\n",
    "plt.imshow(dataset_handler.vis_lanes(final_lanes))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Part III\n",
    "font = {'family': 'serif','color': 'red','weight': 'normal','size': 12}\n",
    "\n",
    "im_out = dataset_handler.vis_object_detection(filtered_detections)\n",
    "\n",
    "for detection, min_distance in zip(filtered_detections, min_distances):\n",
    "    bounding_box = np.asfarray(detection[1:5])\n",
    "    plt.text(bounding_box[0], bounding_box[1] - 20, 'Distance to Impact:' + str(np.round(min_distance, 2)) + ' m', fontdict=font)\n",
    "\n",
    "plt.imshow(im_out)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<font color='blue'>\n",
    "**What you should remember**:\n",
    "- The output of semantic segmentation can be used to estimate drivable space. \n",
    "- Classical computer vision can be used to find lane boundaries.\n",
    "- The output of semantic segmentation can be used to filter out unreliable output from object detection. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Congrats on finishing this assignment! "
   ]
  }
 ],
 "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.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
