main_lane_markings_subm2.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Robust lane finding using advanced computer vision techniques: Second submission\n",
    "\n",
    "\n",
    "### Introduction\n",
    "Lane finding is crucial for developing algorithms for autonomous driving robots or self-driving cars. The lane-finding algorithm must be robust to changing light conditions, weather conditions, other cars/vehicles on the road, curvature of road and type of road itself. In this post, we present an algorithm based on advanced computer vision techniques to identify left and right lanes from dash-mounted camera video. This is second submission for the Advanced lane finding project of [Udacity's Self-driving car nanodegree](https://www.udacity.com/drive). The objective in this project was to apply advanced computer vision techniques to detect lanes and locate the car between the two lanes and compute curvature of the road. The final outcome is illustrated in the figure below. \n",
    "\n",
    "<img src='images/car_init_final.png'>\n",
    "\n",
    "In the notebook below, we will go over the steps taken to go from the image in the left panel to the image in the right panel. \n",
    "\n",
    "### The algorithm\n",
    "\n",
    "The algorithm is divided into two steps, in the first step we apply a perspective transform and compute a lane mask to identify potential locations of lane in an image, and in the next step we combine the lane mask information with previous frame information to compute the final lane. The second step is performed to discard effects of noisy or \n",
    "\n",
    "#### Part 1: Get lane mask\n",
    "\n",
    "Figure below presents the steps involved in obtaining lane masks from the original image. The steps are divided as follows,\n",
    "\n",
    "1. Read and undistort image: In this step, a new image is read by the program and the image is undistorted using precomputed camera distortion matrices. \n",
    "2. Perspective transform: Read in new image and apply perspective transform. Perspective transformation gives us bird's eye view of the road, this makes further processing easier as any irrelevant information about background is removed from the warped image. \n",
    "3. Color Masks: Once we obtain the perspective transform, we next apply color masks to identify yellow and white pixels in the image. Color masks are applied after converting the image from RGB to HSV space. HSV space is more suited for identifying colors as it segements the colors into the color them selves (Hue), the ammount of color (Saturation) and brightness (Value). We identify yellow color as the pixels whose HSV-transformed intensities are between \\\\([ 0, 100, 100]\\\\) and \\\\([ 50, 255, 255]\\\\), and white color as the pixels with intensities between \\\\( [20,   0,   180]\\\\) and \\\\([255,  80, 255] \\\\).\n",
    "4. Sobel Filters: In addition to the color masks, we apply sobel filters to detect edges. We apply sobel filters on L and S channels of image, as these were found to be robust to color and lighting variations. After multiple trial and error, we decided to use the magnitude of gradient along x- and y- directions with thresholds of 50 to 200 as good candidates to identify the edges. \n",
    "5. Combine sobel and color masks: In a final step we combined candidate lane pixels from Sobel filters  and color masks to obtain potential lane regions. \n",
    "\n",
    "These  steps are illustrated in the figure below. \n",
    "\n",
    "<img src='images/lane_mask.png'>\n",
    "\n",
    "\n",
    "From above, we get good representation of lane masks. However, these masks are based on yellow and white colors and sobel filter calculations. If there are additional drawings or markings on the road, this algorithm will not give two neat lines as above, we will therefore perform additional analysis to isolate the lane loactions. \n",
    "\n",
    "#### Part 2: Compute lanes \n",
    "\n",
    "We implement different lane calculations for the first frame and subsequent frames. In the first frame, we compute the lanes using computer vision methods, however, in the later frames, we skip these steps. Instead, we place windows of 50 pixel width centered on the lanes computed in the previous frame, and search within these windows. This significanly reduced the computation time, for our algorithm. We were able to achieve 10 Frames/s lane estimation rate. \n",
    "\n",
    "\n",
    "#### Compute lanes for the first frame\n",
    "\n",
    "The next step is to compute lanes for the first image. To do so, we take the lane mask from the previous step, and take only the bottom half of the image. We next use scipy to compute the locations of the peaks corresponding to the left and right lanes. \n",
    "\n",
    "\n",
    "<img src='images/hist_lane1.png'>\n",
    "\n",
    "We then place a window of size 50 pixels centered at these peaks, and search for peaks in the bottom 8th of the image. Next we move up to the next 1/8th of the image and center windows at the peaks detected in the bottom 1/8th of the image. We repeat this process 8 times to cover the entire image. This is illustrated in the figure below. \n",
    "\n",
    "\n",
    "<img src='images/road_slices.png'>\n",
    "\n",
    "\n",
    "In addition to tracking the location of the previous window, we also keep track of the displacement of previous window. In cases where no peaks are found, we place a window centered at the location calculated assuming the location of previous window moved by a precomputed offset. The windows and lanes obtained after this step are shown below.  \n",
    "\n",
    "<img src='images/sliding_window.png'>\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "We next fit a quadratic function with independent variable 'x' and dependent variable 'y' to the points within the line mask using numpy's polyfit function. \n",
    "\n",
    "<img src='images/poly_fit.png'>\n",
    "\n",
    "After computing the lanes, we draw them back on the original undistorted image as follows. \n",
    "\n",
    "<img src='images/lane_draw.png'>\n",
    "\n",
    "\n",
    "\n",
    "#### Part 3: Check lane mask against previous frame, and compute new lane regions. \n",
    "\n",
    "\n",
    "If the current frame is not the first frame, we follow the same steps as part 2 to get the lane masks, however, we introduced additional steps to ensure any error due to incorrectly detected lanes is removed. Lane correction are introduced as, \n",
    "\n",
    "1. Outlier removal 1: If the change in coefficient is above 0.005, the lanes are discarded. This number was obtained empirically. \n",
    "2. Outlier removal 2: If any lane was found with less than 5 pixels, we use the previous line fit coefficients as the coefficients for the current one. \n",
    "3. Smoothing: We smooth the value of the current lane using a first order filter response, as \\\\(coeffs = 0.95*coeff~prev+ 0.05 coeff\\\\). \n",
    "\n",
    "Finally , we use the coefficients of polynomial fit to compute curvatures of the lane, and relative location of the car in the lane. \n",
    "\n",
    "\n",
    "### Reflection: \n",
    "\n",
    "This was a very interesting and fun project to do. The most interesting part was to see how the techniques developed in a previous simpler project applied to a more real-life type scenario. The work on this project is far from over. The current algorithm is not robust enough to generalize to challenge videos, but performs remarkably well. We will go over details of a more robust algorithm in our final report.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#importing some useful packages\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.image as mpimg\n",
    "import numpy as np\n",
    "import cv2\n",
    "import os\n",
    "import pickle \n",
    "from scipy.interpolate import UnivariateSpline\n",
    "from scipy.signal import find_peaks_cwt\n",
    "\n",
    "\n",
    "\n",
    "# Import everything needed to edit/save/watch video clips\n",
    "from moviepy.editor import VideoFileClip\n",
    "from IPython.display import HTML\n",
    "pi = 3.14159\n",
    "data = pickle.load( open( \"camera_calibration.pkl\", \"rb\" ) )\n",
    "mtx_camera = data[0]\n",
    "dist_camera = data[1]\n",
    "\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Define kernel size for filters\n",
    "kernel_size = 5\n",
    "# Define size for sliding windows\n",
    "window_size = 60\n",
    "\n",
    "\n",
    "# do_diagnosis; variable to "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#reading in an image\n",
    "image = mpimg.imread('test_images/test3.jpg')\n",
    "#image = mpimg.imread('test_images/solidWhiteRight.jpg')\n",
    "\n",
    "plt.imshow(image);  #call as plt.imshow(gray, cmap='gray') to show a grayscaled image\n",
    "plt.axis('off');"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "def get_initial_mask(img,window_sz):\n",
    "    \n",
    "    # This function gets the initial mask\n",
    "    \n",
    "    img = gaussian_blur(img,5)\n",
    "    img_size = np.shape(img)\n",
    "    mov_filtsize = img_size[1]/50.\n",
    "    mean_ln = np.mean(img[img_size[0]/2:,:],axis=0)\n",
    "    mean_ln = moving_average(mean_ln,mov_filtsize)\n",
    "    \n",
    "    indexes = find_peaks_cwt(mean_ln,[100], max_distances=[800])\n",
    "\n",
    "    val_ind = np.array([mean_ln[indexes[i]] for i in range(len(indexes)) ])\n",
    "    ind_sorted = np.argsort(-val_ind)\n",
    "\n",
    "    ind_peakR = indexes[ind_sorted[0]]\n",
    "    ind_peakL = indexes[ind_sorted[1]]\n",
    "    if ind_peakR<ind_peakL:\n",
    "        ind_temp = ind_peakR\n",
    "        ind_peakR = ind_peakL\n",
    "        ind_peakL = ind_temp\n",
    "\n",
    "    n_vals = 8\n",
    "    ind_min_L = ind_peakL-window_sz\n",
    "    ind_max_L = ind_peakL+window_sz\n",
    "\n",
    "    ind_min_R = ind_peakR-window_sz\n",
    "    ind_max_R = ind_peakR+window_sz\n",
    "\n",
    "    mask_L_i = np.zeros_like(img)\n",
    "    mask_R_i = np.zeros_like(img)\n",
    "\n",
    "    ind_peakR_prev = ind_peakR\n",
    "    ind_peakL_prev = ind_peakL\n",
    "    \n",
    "    # Split image into 8 parts and compute histogram on each part\n",
    "    \n",
    "    for i in range(8):\n",
    "        img_y1 = img_size[0]-img_size[0]*i/8\n",
    "        img_y2 = img_size[0]-img_size[0]*(i+1)/8\n",
    "    \n",
    "        mean_lane_y = np.mean(img[img_y2:img_y1,:],axis=0)\n",
    "        mean_lane_y = moving_average(mean_lane_y,mov_filtsize)\n",
    "        indexes = find_peaks_cwt(mean_lane_y,[100], max_distances=[800])\n",
    "        \n",
    "        if len(indexes)>1.5:\n",
    "            val_ind = np.array([mean_ln[indexes[i]] for i in range(len(indexes)) ])\n",
    "            ind_sorted = np.argsort(-val_ind)\n",
    "\n",
    "            ind_peakR = indexes[ind_sorted[0]]\n",
    "            ind_peakL = indexes[ind_sorted[1]]\n",
    "            if ind_peakR<ind_peakL:\n",
    "                ind_temp = ind_peakR\n",
    "                ind_peakR = ind_peakL\n",
    "                ind_peakL = ind_temp\n",
    "            \n",
    "        else:\n",
    "        # If no pixels are found, use previous ones. \n",
    "            if len(indexes)==1:\n",
    "                if (np.abs(indexes[0]-ind_peakR_prev)<np.abs(indexes[0]-ind_peakL_prev)):\n",
    "                    ind_peakR = indexes[0]\n",
    "                    ind_peakL = ind_peakL_prev\n",
    "                else:\n",
    "                    ind_peakL = indexes[0]\n",
    "                    ind_peakR = ind_peakR_prev\n",
    "            else:\n",
    "                ind_peakL = ind_peakL_prev\n",
    "                ind_peakR = ind_peakR_prev\n",
    "            \n",
    "            \n",
    "        # If new center is more than 60pixels away, use previous\n",
    "        # Outlier rejection\n",
    "        if np.abs(ind_peakL-ind_peakL_prev)>=60:\n",
    "            ind_peakL = ind_peakL_prev\n",
    "\n",
    "        if np.abs(ind_peakR-ind_peakR_prev)>=60:\n",
    "            ind_peakR = ind_peakR_prev\n",
    "            \n",
    "    \n",
    "            \n",
    "        mask_L_i[img_y2:img_y1,ind_peakL-window_sz:ind_peakL+window_sz] = 1.\n",
    "        mask_R_i[img_y2:img_y1,ind_peakR-window_sz:ind_peakR+window_sz] = 1.\n",
    "        \n",
    "        ind_peakL_prev = ind_peakL\n",
    "        ind_peakR_prev = ind_peakR\n",
    "        \n",
    "    return mask_L_i,mask_R_i\n",
    "\n",
    "\n",
    "\n",
    "def get_mask_poly(img,poly_fit,window_sz):\n",
    "    \n",
    "    # This function returns masks for points used in computing polynomial fit. \n",
    "    mask_poly = np.zeros_like(img)\n",
    "    img_size = np.shape(img)\n",
    "\n",
    "    poly_pts = []\n",
    "    pt_y_all = []\n",
    "\n",
    "    for i in range(8):\n",
    "        img_y1 = img_size[0]-img_size[0]*i/8\n",
    "        img_y2 = img_size[0]-img_size[0]*(i+1)/8\n",
    "\n",
    "        pt_y = (img_y1+img_y2)/2\n",
    "        pt_y_all.append(pt_y)\n",
    "        poly_pt = np.round(poly_fit[0]*pt_y**2 + poly_fit[1]*pt_y + poly_fit[2])\n",
    "    \n",
    "        poly_pts.append(poly_pt)\n",
    "    \n",
    "        mask_poly[img_y2:img_y1,poly_pt-window_sz:poly_pt+window_sz] = 1.     \n",
    "\n",
    "    return mask_poly, np.array(poly_pts),np.array(pt_y_all)\n",
    "    \n",
    "\n",
    "def get_val(y,pol_a):\n",
    "    # Returns value of a quadratic polynomial \n",
    "    return pol_a[0]*y**2+pol_a[1]*y+pol_a[2]\n",
    "\n",
    "def draw_pw_lines(img,pts,color):\n",
    "    # This function draws lines connecting 10 points along the polynomial\n",
    "    pts = np.int_(pts)\n",
    "    for i in range(10):\n",
    "        x1 = pts[0][i][0]\n",
    "        y1 = pts[0][i][1]\n",
    "        x2 = pts[0][i+1][0]\n",
    "        y2 = pts[0][i+1][1]\n",
    "        cv2.line(img, (x1, y1), (x2, y2),color,50)\n",
    "        \n",
    "def undistort_image(img, mtx, dist):\n",
    "    # Function to undistort image\n",
    "    undist_img = cv2.undistort(img, mtx, dist, None, mtx)\n",
    "    return undist_img\n",
    "def gaussian_blur(img, kernel=5):\n",
    "    # Function to smooth image\n",
    "    blur = cv2.GaussianBlur(img,(kernel,kernel),0)\n",
    "    return blur\n",
    "\n",
    "def abs_sobel_thresh(img, orient='x', sobel_kernel=3, thresh=(0, 255)):\n",
    "    # Calculate directional gradient\n",
    "    # Apply threshold\n",
    "    if orient=='x':\n",
    "        img_s = cv2.Sobel(img,cv2.CV_64F, 1, 0)\n",
    "    else:\n",
    "        img_s = cv2.Sobel(img,cv2.CV_64F, 0, 1)\n",
    "    img_abs = np.absolute(img_s)\n",
    "    img_sobel = np.uint8(255*img_abs/np.max(img_abs))\n",
    "    \n",
    "    binary_output = 0*img_sobel\n",
    "    binary_output[(img_sobel >= thresh[0]) & (img_sobel <= thresh[1])] = 1\n",
    "    return binary_output\n",
    "\n",
    "def mag_thresh(img, sobel_kernel=3, thresh=(0, 255)):\n",
    "    # Calculate gradient magnitude\n",
    "    # Apply threshold\n",
    "    img_sx = cv2.Sobel(img,cv2.CV_64F, 1, 0)\n",
    "    img_sy = cv2.Sobel(img,cv2.CV_64F, 0, 1)\n",
    "    \n",
    "    img_s = np.sqrt(img_sx**2 + img_sy**2)\n",
    "    img_s = np.uint8(img_s*255/np.max(img_s))\n",
    "    binary_output = 0*img_s\n",
    "    binary_output[(img_s>=thresh[0]) & (img_s<=thresh[1]) ]=1\n",
    "    return binary_output\n",
    "\n",
    "def dir_threshold(img, sobel_kernel=3, thresh=(0, np.pi/2)):\n",
    "    # Calculate gradient direction\n",
    "    # Apply threshold\n",
    "    img_sx = cv2.Sobel(img,cv2.CV_64F,1,0, ksize=sobel_kernel)\n",
    "    img_sy = cv2.Sobel(img,cv2.CV_64F,0,1, ksize=sobel_kernel)\n",
    "    \n",
    "    grad_s = np.arctan2(np.absolute(img_sy), np.absolute(img_sx))\n",
    "    \n",
    "    binary_output = 0*grad_s # Remove this line\n",
    "    binary_output[(grad_s>=thresh[0]) & (grad_s<=thresh[1])] = 1\n",
    "    return binary_output\n",
    "    \n",
    "def GaussianC_Adaptive_Threshold(img,kernel,cut_val):\n",
    "    # Gaussian adaptive thresholding (NOT USED )\n",
    "    img_cut = cv2.adaptiveThreshold(img,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\\\n",
    "            cv2.THRESH_BINARY,kernel,cut_val)\n",
    "    return img_cut\n",
    "\n",
    "def warp_image(img,src,dst,img_size):\n",
    "    # Apply perspective transform\n",
    "    M = cv2.getPerspectiveTransform(src, dst)\n",
    "    warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)\n",
    "    Minv = cv2.getPerspectiveTransform(dst, src)\n",
    "\n",
    "    return warped,M,Minv\n",
    "\n",
    "def sobel_combined(image):\n",
    "    # Combine sobel masks.\n",
    "    img_g_mag = mag_thresh(image,3,(20,150))\n",
    "    img_d_mag = dir_threshold(image,3,(.6,1.1))\n",
    "    img_abs_x = abs_sobel_thresh(image,'x',5,(50,200))\n",
    "    img_abs_y = abs_sobel_thresh(image,'y',5,(50,200))\n",
    "    sobel_combined = np.zeros_like(img_d_mag)\n",
    "    sobel_combined[((img_abs_x == 1) & (img_abs_y == 1)) | \\\n",
    "               ((img_g_mag == 1) & (img_d_mag == 1))] = 1\n",
    "    return sobel_combined\n",
    "\n",
    "\n",
    "def color_mask(hsv,low,high):\n",
    "    # Takes in low and high values and returns mask\n",
    "    mask = cv2.inRange(hsv, low, high)\n",
    "    return mask\n",
    "\n",
    "def apply_color_mask(hsv,img,low,high):\n",
    "    # Takes in color mask and returns image with mask applied.\n",
    "    mask = cv2.inRange(hsv, low, high)\n",
    "    res = cv2.bitwise_and(img,img, mask= mask)\n",
    "    return res\n",
    "\n",
    "def moving_average(a, n=3):\n",
    "    # Moving average\n",
    "    ret = np.cumsum(a, dtype=float)\n",
    "    ret[n:] = ret[n:] - ret[:-n]\n",
    "    return ret[n - 1:] / n\n",
    "\n",
    "def get_curvature(pol_a,y_pt):\n",
    "    # Returns curvature of a quadratic\n",
    "    A = pol_a[0]\n",
    "    B = pol_a[1]\n",
    "    R_curve = (1+(2*A*y_pt+B)**2)**1.5/2/A\n",
    "    return R_curve\n",
    "\n",
    "def stack_arr(arr):\n",
    "    # Stacks 1-channel array into 3-channel array to allow plotting\n",
    "    return np.stack((arr, arr,arr), axis=2)\n",
    "\n",
    "\n",
    "def apply_perspective_transform(image):\n",
    "    # Applies bird-eye perspective transform to an image\n",
    "    img_size = image.shape\n",
    "    ht_window = np.uint(img_size[0]/1.5)\n",
    "    hb_window = np.uint(img_size[0])\n",
    "    c_window = np.uint(img_size[1]/2)\n",
    "    ctl_window = c_window - .25*np.uint(img_size[1]/2)\n",
    "    ctr_window = c_window + .25*np.uint(img_size[1]/2)\n",
    "    cbl_window = c_window - 1*np.uint(img_size[1]/2)\n",
    "    cbr_window = c_window + 1*np.uint(img_size[1]/2)\n",
    "    src = np.float32([[cbl_window,hb_window],[cbr_window,hb_window],\n",
    "                      [ctr_window,ht_window],[ctl_window,ht_window]])\n",
    "    dst = np.float32([[0,img_size[0]],[img_size[1],img_size[0]],\n",
    "                  [img_size[1],0],[0,0]])\n",
    "    \n",
    "    warped,M_warp,Minv_warp = warp_image(image,src,dst,(img_size[1],img_size[0])) # returns birds eye image\n",
    "    return warped,M_warp,Minv_warp\n",
    "\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def pipeline_process_highway(image):\n",
    "    \n",
    "    global left_fit_prev   \n",
    "    global right_fit_prev\n",
    "    global col_R_prev\n",
    "    global col_L_prev\n",
    "    global set_prev\n",
    "    global mask_poly_L\n",
    "    global mask_poly_R\n",
    "    \n",
    "\n",
    "    # Undistort image\n",
    "\n",
    "    image = undistort_image(image, mtx_camera , dist_camera )\n",
    "    image = gaussian_blur(image, kernel=5)\n",
    "    img_size = np.shape(image)\n",
    "    \n",
    "    # Define window for perspective transform\n",
    "    warped,M_warp,Minv_warp = apply_perspective_transform(image)\n",
    "    image_HSV = cv2.cvtColor(warped,cv2.COLOR_RGB2HSV)\n",
    "\n",
    "    # Define color ranges and apply color mask\n",
    "    yellow_hsv_low  = np.array([ 0, 100, 100])\n",
    "    yellow_hsv_high = np.array([ 50, 255, 255])\n",
    "\n",
    "    white_hsv_low  = np.array([  20,   0,   180])\n",
    "    white_hsv_high = np.array([ 255,  80, 255])\n",
    "    # get yellow and white masks \n",
    "    mask_yellow = color_mask(image_HSV,yellow_hsv_low,yellow_hsv_high)\n",
    "    mask_white = color_mask(image_HSV,white_hsv_low,white_hsv_high)\n",
    "    # Combine white and yellow masks into 1\n",
    "    mask_lane = cv2.bitwise_or(mask_yellow,mask_white) \n",
    "    \n",
    "    # Convert image to HLS scheme\n",
    "    image_HLS = cv2.cvtColor(warped,cv2.COLOR_RGB2HLS)\n",
    "\n",
    "    # Apply sobel filters on L and S channels.\n",
    "    img_gs = image_HLS[:,:,1]\n",
    "    img_abs_x = abs_sobel_thresh(img_gs,'x',5,(50,225))\n",
    "    img_abs_y = abs_sobel_thresh(img_gs,'y',5,(50,225))\n",
    "    wraped2 = np.copy(cv2.bitwise_or(img_abs_x,img_abs_y))\n",
    "    \n",
    "    img_gs = image_HLS[:,:,2]\n",
    "    img_abs_x = abs_sobel_thresh(img_gs,'x',5,(50,255))\n",
    "    img_abs_y = abs_sobel_thresh(img_gs,'y',5,(50,255))\n",
    "    wraped3 = np.copy(cv2.bitwise_or(img_abs_x,img_abs_y))\n",
    "    \n",
    "\n",
    "    # Combine sobel filter information from L and S channels.\n",
    "    image_cmb = cv2.bitwise_or(wraped2,wraped3)\n",
    "    image_cmb = gaussian_blur(image_cmb,25)\n",
    "    \n",
    "\n",
    "    # Combine masks from sobel and color masks.\n",
    "\n",
    "    image_cmb1 = np.zeros_like(image_cmb)\n",
    "    image_cmb1[(mask_lane>=.5)|(image_cmb>=.5)]=1\n",
    "    \n",
    "    \n",
    "    # If this is first frame, get new mask.\n",
    "    if set_prev == 0:\n",
    "        image_cmb1 = gaussian_blur(image_cmb1,5)\n",
    "        mask_poly_L,mask_poly_R = get_initial_mask(image_cmb1,40)\n",
    "\n",
    "        \n",
    "        \n",
    "    # Define all colors as white to start.         \n",
    "    col_R = (255,255,255)\n",
    "    col_L = (255,255,255)\n",
    "    col_R = (255,255,255)\n",
    "    col_L = (255,255,255)\n",
    "    \n",
    "    # Apply mask to sobel images and compute polynomial fit for left. \n",
    "    img_L = np.copy(image_cmb1)\n",
    "    img_L = cv2.bitwise_and(image_cmb1,image_cmb1,\n",
    "                                mask = mask_poly_L)\n",
    "    vals = np.argwhere(img_L>.5)\n",
    "    if len(vals)<5: ## If less than 5 points \n",
    "        left_fit = left_fit_prev\n",
    "        col_L = col_L_prev\n",
    "    else:\n",
    "        all_x = vals.T[0]\n",
    "        all_y =vals.T[1]\n",
    "        left_fit = np.polyfit(all_x, all_y, 2)\n",
    "        if np.sum(cv2.bitwise_and(img_L,mask_yellow))>1000:\n",
    "            col_L = (255,255,0)\n",
    "            \n",
    "    # Apply mask to sobel images and compute polynomial fit for right. \n",
    "\n",
    "    img_R = np.copy(image_cmb1)\n",
    "    img_R = cv2.bitwise_and(image_cmb1,image_cmb1,\n",
    "                                mask = mask_poly_R)\n",
    "    vals = np.argwhere(img_R>.5)\n",
    "        \n",
    "    if len(vals)<5:\n",
    "        right_fit = right_fit_prev\n",
    "        col_R = col_R_prev\n",
    "    else:\n",
    "        all_x = vals.T[0]\n",
    "        all_y =vals.T[1]\n",
    "        right_fit = np.polyfit(all_x, all_y, 2)\n",
    "        if np.sum(cv2.bitwise_and(img_R,mask_yellow))>1000:\n",
    "            col_R = (255,255,0)\n",
    "    \n",
    "    \n",
    "    ## assign initial mask, and save coefficient values for next frame\n",
    "            \n",
    "    if set_prev == 0:\n",
    "        set_prev = 1\n",
    "        right_fit_prev = right_fit\n",
    "        left_fit_prev  = left_fit\n",
    "    \n",
    "       \n",
    "    ## Check error between current coefficient and on from previous frame\n",
    "    err_p_R = np.sum((right_fit[0]-right_fit_prev[0])**2) #/np.sum(right_fit_prev[0]**2)\n",
    "    err_p_R = np.sqrt(err_p_R)\n",
    "    if err_p_R>.0005:\n",
    "        right_fit = right_fit_prev\n",
    "        col_R = col_R_prev\n",
    "    else:\n",
    "        right_fit = .05*right_fit+.95*right_fit_prev\n",
    "        \n",
    "    ## Check error between current coefficient and on from previous frame\n",
    "    err_p_L = np.sum((left_fit[0]-left_fit_prev[0])**2) #/np.sum(right_fit_prev[0]**2)\n",
    "    err_p_L = np.sqrt(err_p_L)\n",
    "    if err_p_L>.0005:\n",
    "        left_fit =  left_fit_prev\n",
    "        col_L = col_L_prev\n",
    "    else:\n",
    "        left_fit =  .05* left_fit+.95* left_fit_prev\n",
    "    \n",
    "\n",
    "    ## Compute lane mask for future frame \n",
    "    mask_poly_L,left_pts,img_pts = get_mask_poly(image_cmb1,left_fit,window_size)\n",
    "    mask_poly_R,right_pts,img_pts = get_mask_poly(image_cmb1,right_fit,window_size)\n",
    "     \n",
    "        \n",
    "    ## Compute lanes\n",
    "        \n",
    "    right_y = np.arange(11)*img_size[0]/10\n",
    "    right_fitx = right_fit[0]*right_y**2 + right_fit[1]*right_y + right_fit[2]\n",
    "\n",
    "    left_y = np.arange(11)*img_size[0]/10\n",
    "    left_fitx = left_fit[0]*left_y**2 + left_fit[1]*left_y + left_fit[2]\n",
    "    \n",
    "    warp_zero = np.zeros_like(image_cmb1).astype(np.uint8)\n",
    "    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))\n",
    "\n",
    "    # Recast the x and y points into usable format for cv2.fillPoly()\n",
    "    pts_left = np.array([np.transpose(np.vstack([left_fitx, left_y]))])\n",
    "    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, right_y])))])\n",
    "    pts = np.hstack((pts_left, pts_right))\n",
    "    \n",
    "\n",
    "    ## Compute intercepts\n",
    "    left_bot = get_val(img_size[0],left_fit)\n",
    "    right_bot = get_val(img_size[0],right_fit)\n",
    "    \n",
    "    ## Compute center location\n",
    "    val_center = (left_bot+right_bot)/2.0\n",
    "    \n",
    "    ## Compute lane offset\n",
    "    dist_offset = val_center - img_size[1]/2\n",
    "    dist_offset = np.round(dist_offset/2.81362,2)\n",
    "    str_offset = 'Lane deviation: ' + str(dist_offset) + ' cm.'\n",
    "    \n",
    "    if dist_offset>30:\n",
    "        cv2.fillPoly(color_warp, np.int_([pts]), (255,0, 0))\n",
    "    else:\n",
    "        cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 255))\n",
    "        \n",
    "\n",
    "    # Draw the lane onto the warped blank image    \n",
    "    draw_pw_lines(color_warp,np.int_(pts_left),col_L)\n",
    "    draw_pw_lines(color_warp,np.int_(pts_right),col_R)\n",
    "    # Warp the blank back to original image space using inverse perspective matrix (Minv)\n",
    "    \n",
    "    newwarp = cv2.warpPerspective(color_warp, Minv_warp, (image.shape[1], image.shape[0])) \n",
    "    # Combine the result with the original image\n",
    "    result = cv2.addWeighted(image, 1, newwarp, 0.5, 0)\n",
    "    \n",
    "    left_curve = get_curvature(left_fit,img_size[0]/2)\n",
    "    Right_curve = get_curvature(right_fit,img_size[0]/2)\n",
    "    \n",
    "    str_curv = 'Curvature: Right = ' + str(np.round(Right_curve,2)) + ', Left = ' + str(np.round(left_curve,2)) \n",
    "    # Change color if distance is more than 30 cm\n",
    "    font = cv2.FONT_HERSHEY_COMPLEX    \n",
    "    if dist_offset<30:\n",
    "        cv2.putText(result, str_curv, (30, 60), font, 1, (0,255,0), 2)\n",
    "        cv2.putText(result, str_offset, (30, 90), font, 1, (0,255,0), 2)\n",
    "    else:\n",
    "        cv2.putText(result, str_curv, (30, 60), font, 1, (255,0,0), 2)\n",
    "        cv2.putText(result, str_offset, (30, 90), font, 1, (255,0,0), 2)\n",
    "    \n",
    "    \n",
    "    \n",
    "    right_fit_prev = right_fit\n",
    "    left_fit_prev  = left_fit\n",
    "    col_R_prev = col_R\n",
    "    col_L_prev = col_L\n",
    "    \n",
    "    \n",
    "    #return result    # using cv2 for drawing text in diagnostic pipeline.\n",
    "    \n",
    "    if do_diagnosis == 1:\n",
    "        font = cv2.FONT_HERSHEY_COMPLEX\n",
    "        middlepanel = np.zeros((120, 1280, 3), dtype=np.uint8)\n",
    "        cv2.putText(middlepanel, str_curv, (30, 60), font, 1, (255,0,0), 2)\n",
    "        cv2.putText(middlepanel, str_offset, (30, 90), font, 1, (255,0,0), 2)\n",
    "    \n",
    "        # assemble the screen example\n",
    "        diagScreen = np.zeros((1080, 1920, 3), dtype=np.uint8)\n",
    "        diagScreen[0:720, 0:1280] = result\n",
    "        diagScreen[0:240, 1280:1600] = cv2.resize(warped, (320,240), interpolation=cv2.INTER_AREA) \n",
    "        diagScreen[0:240, 1600:1920] = cv2.resize(stack_arr(mask_lane), (320,240), interpolation=cv2.INTER_AREA)\n",
    "        diagScreen[240:480, 1280:1600] = cv2.resize(apply_color_mask(image_HSV,warped,yellow_hsv_low,yellow_hsv_high), (320,240), interpolation=cv2.INTER_AREA)\n",
    "        diagScreen[240:480, 1600:1920] = cv2.resize(apply_color_mask(image_HSV,warped,white_hsv_low,white_hsv_high), (320,240), interpolation=cv2.INTER_AREA)*4\n",
    "        diagScreen[600:1080, 1280:1920] = cv2.resize(color_warp, (640,480), interpolation=cv2.INTER_AREA)*4\n",
    "        diagScreen[720:840, 0:1280] = middlepanel\n",
    "        diagScreen[840:1080, 0:320] = cv2.resize(newwarp, (320,240), interpolation=cv2.INTER_AREA)\n",
    "        #diagScreen[840:1080, 320:640] = cv2.resize(stack_arr(255*mask_poly_L), (320,240), interpolation=cv2.INTER_AREA)\n",
    "        #diagScreen[840:1080, 640:960] = cv2.resize(stack_arr(255*mask_poly_R), (320,240), interpolation=cv2.INTER_AREA)\n",
    "        #diagScreen[840:1080, 960:1280] = cv2.resize(stack_arr(255*image_cmb1), (320,240), interpolation=cv2.INTER_AREA)\n",
    "    \n",
    "        diagScreen[840:1080, 320:640] = cv2.resize(stack_arr(255*image_cmb1), (320,240), interpolation=cv2.INTER_AREA)\n",
    "        diagScreen[840:1080, 640:960] = cv2.resize(stack_arr(255*mask_poly_L+255*mask_poly_R), (320,240), interpolation=cv2.INTER_AREA)\n",
    "        diagScreen[840:1080, 960:1280] = cv2.resize(stack_arr(255*cv2.bitwise_and(image_cmb1,image_cmb1,\n",
    "                                                                              mask=mask_poly_L+mask_poly_R)),\n",
    "                                                (320,240), interpolation=cv2.INTER_AREA)\n",
    "        return diagScreen\n",
    "    else:\n",
    "        return result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "do_diagnosis = 0\n",
    "set_prev = 0\n",
    "\n",
    "result_pipe = pipeline_process_highway(image)\n",
    "plt.imshow(result_pipe)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "set_prev = 0\n",
    "do_diagnosis = 1\n",
    "\n",
    "\n",
    "result_pipe = pipeline_process_diagnosis(image)\n",
    "plt.imshow(result_pipe)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Import everything needed to edit/save/watch video clips\n",
    "from moviepy.editor import VideoFileClip\n",
    "from IPython.display import HTML"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#left_fit_prev = np.array([0,0,0])\n",
    "#right_fit_prev = np.array([0,0,0])\n",
    "set_prev = 0\n",
    "do_diagnosis = 0\n",
    "challenge_output = 'challenge_video_output.mp4'\n",
    "clip2 = VideoFileClip(\"challenge_video.mp4\");\n",
    "white_clip = clip2.fl_image(pipeline_process_highway) #NOTE: this function expects color images!!\n",
    "%time white_clip.write_videofile(challenge_output, audio=False);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#left_fit_prev = np.array([0,0,0])\n",
    "#right_fit_prev = np.array([0,0,0])\n",
    "set_prev = 0\n",
    "do_diagnosis = 0\n",
    "\n",
    "project_output = 'project_video_output.mp4'\n",
    "clip1 = VideoFileClip(\"project_video.mp4\");\n",
    "white_clip = clip1.fl_image(pipeline_process_highway) #NOTE: this function expects color images!!\n",
    "%time white_clip.write_videofile(project_output, audio=False);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "#left_fit_prev = np.array([0,0,0])\n",
    "#right_fit_prev = np.array([0,0,0])\n",
    "set_prev = 0\n",
    "do_diagnosis = 1\n",
    "\n",
    "project_output_diag = 'project_video_output_diag.mp4'\n",
    "clip3 = VideoFileClip(\"project_video.mp4\");\n",
    "white_clip = clip3.fl_image(pipeline_process_highway) #NOTE: this function expects color images!!\n",
    "%time white_clip.write_videofile(project_output_diag, audio=False);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#left_fit_prev = np.array([0,0,0])\n",
    "#right_fit_prev = np.array([0,0,0])\n",
    "set_prev = 0\n",
    "do_diagnosis = 1\n",
    "\n",
    "challenge_output_diag = 'challenge_video_output_diag.mp4'\n",
    "clip4 = VideoFileClip(\"challenge_video.mp4\");\n",
    "white_clip = clip4.fl_image(pipeline_process_highway) #NOTE: this function expects color images!!\n",
    "%time white_clip.write_videofile(challenge_output_diag, audio=False);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}