CarND-Advanced-Lane-Lines.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Advanced Lane Finding Project\n",
    "\n",
    "The goals / steps of this project are the following:\n",
    "\n",
    "* Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.\n",
    "* Apply a distortion correction to raw images.\n",
    "* Use color transforms, gradients, etc., to create a thresholded binary image.\n",
    "* Apply a perspective transform to rectify binary image (\"birds-eye view\").\n",
    "* Detect lane pixels and fit to find the lane boundary.\n",
    "* Determine the curvature of the lane and vehicle position with respect to center.\n",
    "* Warp the detected lane boundaries back onto the original image.\n",
    "* Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.\n",
    "---"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "import numpy as np                 # NumPy\n",
    "import cv2                         # openCV\n",
    "import glob                        # Filename pattern matching\n",
    "import matplotlib.pyplot as plt    # 2D plotting\n",
    "import matplotlib.image as mpimg\n",
    "\n",
    "# Interactive plotting in separate window\n",
    "#%matplotlib qt\n",
    "# Visualizations will be shown in the notebook\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Compute the camera calibration points using chessboard images"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "def get_3d2d_points(do_plot=False, do_file=False):\n",
    "    # Prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)\n",
    "    objp = np.zeros((6*9, 3), np.float32)\n",
    "    objp[:,:2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)\n",
    "\n",
    "    # Arrays to store object points and image points from all the images\n",
    "    objpoints = []                     # 3D points in real world space\n",
    "    imgpoints = []                     # 2D points in image plain\n",
    "\n",
    "    # List of calibration images\n",
    "    images = glob.glob('camera_cal/calibration*.jpg')\n",
    "    print('Num of calibration images: {0}'.format(len(images)))\n",
    "\n",
    "    # Step through the list and search for chessboard corners\n",
    "    for img_id, fname in enumerate(images):\n",
    "        img = cv2.imread(fname)\n",
    "        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "        # Find the chessboard corners\n",
    "        # http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#findchessboardcorners\n",
    "        # cv2.findChessboardCorners(image, patternSize[, corners[, flags]]) → retval, corners\n",
    "        ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)\n",
    "        # If found - add object points, add image points\n",
    "        if ret == True:\n",
    "            objpoints.append(objp)\n",
    "            imgpoints.append(corners)\n",
    "            # Draw and display the corners\n",
    "            cv2.drawChessboardCorners(img, (9, 6), corners, ret)\n",
    "            # Draw the plot\n",
    "            if do_plot:\n",
    "                plt.imshow(img)\n",
    "                plt.show()\n",
    "            # Save to the file\n",
    "            if do_file:\n",
    "                write_name = 'corners_' + str(img_id) + '.jpg'\n",
    "                cv2.imwrite(write_name, img)\n",
    "    return objpoints, imgpoints"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Distortion correction"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "import pickle\n",
    "\n",
    "def pickle_dump(mtx, dist):\n",
    "    # Save the camera calibration result for later use (we won't worry about rvecs / tvecs)\n",
    "    dist_pickle = {}\n",
    "    dist_pickle[\"mtx\"] = mtx\n",
    "    dist_pickle[\"dist\"] = dist\n",
    "    pickle.dump(dist_pickle, open('wide_dist_pickle.p', 'wb'))\n",
    "    \n",
    "def pickle_load():\n",
    "    # Getting back the camera calibration result:\n",
    "    with open('wide_dist_pickle.p', 'rb') as f:\n",
    "        dist_pickle = pickle.load(f)\n",
    "        return dist_pickle['mtx'], dist_pickle['dist']\n",
    "    \n",
    "def calibrate_camera(img):\n",
    "    img_size = (img.shape[1], img.shape[0])\n",
    "    # Do camera calibration given object points and image points\n",
    "    objpoints, imgpoints = get_3d2d_points(do_plot=False, do_file=False)\n",
    "    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_size, None, None)\n",
    "    # Save the camera calibration result\n",
    "    pickle_dump(mtx, dist)\n",
    "    return mtx, dist"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Color/gradient threshold\n",
    "\n",
    "Combine color and gradient thresholds to generate a binary image where the lane lines are clearly visible.\n",
    "\n",
    "Output should be an array of the same size as the input image. The output array elements should be 1 where gradients were in the threshold range, and 0 everywhere else."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# For universal plotting of results\n",
    "def plot_row2(img1, img2, label_1, label_2, graysc=True):\n",
    "    # Plot the result (1 row with 2 images)\n",
    "    f, (ax1, ax2) = plt.subplots(1, 2, figsize=(18, 6))\n",
    "    f.tight_layout()\n",
    "    if graysc:\n",
    "        ax1.imshow(img1, cmap='gray')\n",
    "    else:\n",
    "        ax1.imshow(img1)\n",
    "    ax1.set_title(label_1, fontsize=16)\n",
    "    ax2.imshow(img2, cmap='gray')\n",
    "    ax2.set_title(label_2, fontsize=16)\n",
    "    plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# Apply Sobel (Calculate directional gradient and apply gradient threshold)\n",
    "def abs_sobel_thresh(img, orient='x', sobel_kernel=3, thresh=(0, 255)):\n",
    "    # Convert to HLS and take S channel\n",
    "    img_trans = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)\n",
    "    img_trans = img_trans[:,:,2]\n",
    "    # Take the derivative in x or y given orient = 'x' or 'y'\n",
    "    if orient == 'x':\n",
    "        sobel = cv2.Sobel(img_trans, cv2.CV_64F, 1, 0, ksize=sobel_kernel)\n",
    "    elif orient == 'y':\n",
    "        sobel = cv2.Sobel(img_trans, cv2.CV_64F, 0, 1, ksize=sobel_kernel) \n",
    "    # Take the absolute value of the derivative or gradient\n",
    "    abs_sobel = np.absolute(sobel)\n",
    "    # Scale to 8-bit (0 - 255) then convert to type = np.uint8\n",
    "    scaled_sobel = np.uint8(255 * abs_sobel / np.max(abs_sobel))\n",
    "    # Create a mask of 1's where the scaled gradient magnitude \n",
    "    # is > thresh_min and < thresh_max\n",
    "    binary_output = np.zeros_like(scaled_sobel)\n",
    "    binary_output[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1\n",
    "    # Return mask as binary_output image\n",
    "    return binary_output\n",
    "\n",
    "# Return the magnitude of the gradient for a given sobel kernel \n",
    "# size and threshold values in both x and y\n",
    "def mag_thresh(img, sobel_kernel=3, mag_thresh=(0, 255)):\n",
    "    # Convert to HLS and take S channel\n",
    "    img_trans = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)\n",
    "    img_trans = img_trans[:,:,2]\n",
    "    # Take the gradient in x and y separately\n",
    "    sobel_x = cv2.Sobel(img_trans, cv2.CV_64F, 1, 0, ksize=sobel_kernel)\n",
    "    sobel_y = cv2.Sobel(img_trans, cv2.CV_64F, 0, 1, ksize=sobel_kernel)\n",
    "    # Calculate the gradient magnitude\n",
    "    gradmag = np.sqrt(sobel_x**2 + sobel_y**2)\n",
    "    # Rescale to 8 bit\n",
    "    scale_factor = np.max(gradmag)/255 \n",
    "    gradmag = (gradmag/scale_factor).astype(np.uint8)\n",
    "    # Create a binary mask where mag thresholds are met\n",
    "    binary_output = np.zeros_like(gradmag)\n",
    "    binary_output[(gradmag >= mag_thresh[0]) & (gradmag <= mag_thresh[1])] = 1\n",
    "    # Return mask as binary_output image\n",
    "    return binary_output\n",
    "\n",
    "# Calculate gradient direction and apply threshold\n",
    "def dir_threshold(img, sobel_kernel=3, thresh=(0, np.pi/2)):\n",
    "    # Convert to HLS and take S channel\n",
    "    img_trans = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)\n",
    "    img_trans = img_trans[:,:,2]\n",
    "    # Take the gradient in x and y separately\n",
    "    sobel_x = cv2.Sobel(img_trans, cv2.CV_64F, 1, 0, ksize=sobel_kernel)\n",
    "    sobel_y = cv2.Sobel(img_trans, cv2.CV_64F, 0, 1, ksize=sobel_kernel)\n",
    "    # Take the absolute value of the x and y gradients\n",
    "    abs_sobel_x = np.absolute(sobel_x)\n",
    "    abs_sobel_y = np.absolute(sobel_y)\n",
    "    # Use np.arctan2(abs_sobel_y, abs_sobimg_transel_x) to calculate the direction of the gradient\n",
    "    absgraddir = np.arctan2(abs_sobel_y, abs_sobel_x)\n",
    "    # Create a binary mask where direction thresholds are met\n",
    "    binary_output = np.zeros_like(absgraddir)\n",
    "    binary_output[(absgraddir >= thresh[0]) & (absgraddir <= thresh[1])] = 1\n",
    "    # Return this mask as binary_output image\n",
    "    return binary_output"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "def combine_sobel_thresholds(img, do_plot=False):\n",
    "    # Gaussian Blur\n",
    "    kernel_size = 5\n",
    "    img = cv2.GaussianBlur(img, (kernel_size, kernel_size), 0)\n",
    "    # Sobel kernel size (choose a larger odd number to smooth gradient measurements)\n",
    "    ksize = 7\n",
    "    # Apply Sobel on x-axis\n",
    "    grad_x_binary = abs_sobel_thresh(img, orient='x', sobel_kernel=ksize, thresh=(10, 255))\n",
    "    # Apply Sobel on y-axis\n",
    "    grad_y_binary = abs_sobel_thresh(img, orient='y', sobel_kernel=ksize, thresh=(60, 255))\n",
    "    # Apply Sobel x and y, compute the magnitude of the gradient and apply a threshold\n",
    "    mag_binary = mag_thresh(img, sobel_kernel=ksize, mag_thresh=(40, 255))\n",
    "    # Apply Sobel x and y, computes the direction of the gradient and apply a threshold\n",
    "    dir_binary = dir_threshold(image, sobel_kernel=ksize, thresh=(0.65, 1.05))\n",
    "    \n",
    "    # Combine the thresholds\n",
    "    combined = np.zeros_like(dir_binary)\n",
    "    combined[((grad_x_binary == 1) & (grad_y_binary == 1)) | ((mag_binary == 1) & (dir_binary == 1))] = 1\n",
    "    \n",
    "    if do_plot:\n",
    "        plot_row2(image, grad_x_binary,      'Original Image (Undistorted)', 'Sobel on x-axis')\n",
    "        plot_row2(grad_y_binary, mag_binary, 'Sobel on y-axis', 'Thresholded Magnitude')\n",
    "        plot_row2(dir_binary, combined,      'Direction of gradient', 'Combined Thresholds')\n",
    "    return combined"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "def color_channel_threshold(img, thresh=(0, 255), do_plot=False):\n",
    "    # Convert to HLS color space and separate the S channel\n",
    "    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)\n",
    "    \n",
    "    # Extract S channel\n",
    "    s_channel = hls[:,:,2]\n",
    "    s_binary = np.zeros_like(s_channel)\n",
    "    s_binary[(s_channel >= thresh[0]) & (s_channel <= thresh[1])] = 1\n",
    "    \n",
    "    # Extract L channel\n",
    "    #l_channel = hls[:,:,1]\n",
    "    #l_binary = np.zeros_like(l_channel)\n",
    "    #l_binary[(l_channel >= thresh[0]) & (l_channel <= thresh[1])] = 1\n",
    "    \n",
    "    # Combine S and L channels\n",
    "    #combined = np.zeros_like(l_binary)\n",
    "    #combined[((s_binary == 1) & (l_binary == 1))] = 1\n",
    "    if do_plot:\n",
    "        plot_row2(image, s_binary, 'Original Image (Undistorted)', 'HLS(S-channel) threshold')\n",
    "    #    plot_row2(image, l_binary, 'Original Image', 'L threshold')\n",
    "    #    plot_row2(image, combined, 'Original Image', 'S and L threshold')\n",
    "    #return combined\n",
    "    return s_binary"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Perspective transform\n",
    "\n",
    "Pick four points in a trapezoidal shape (similar to region masking) that would represent a rectangle when looking down on the road from above.\n",
    "\n",
    "The easiest way to do this is to investigate an image where the lane lines are straight, and find four points lying along the lines that, after perspective transform, make the lines look straight and vertical from a bird's eye view perspective."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# Applies an image mask\n",
    "# Only keeps the region of the image defined by the polygon formed from `vertices`.\n",
    "# The rest of the image is set to black.\n",
    "def region_of_interest(img, vertices):\n",
    "    # Defining a blank mask to start with\n",
    "    mask = np.zeros_like(img)\n",
    "    ignore_mask_color = 255\n",
    "    # Fill pixels inside the polygon defined by \"vertices\" with the fill color    \n",
    "    cv2.fillPoly(mask, vertices, ignore_mask_color)\n",
    "    # Return the image only where mask pixels are nonzero\n",
    "    masked_image = cv2.bitwise_and(img, mask)\n",
    "    return masked_image\n",
    "\n",
    "def perspective_transform(img, inv=False):\n",
    "    # Define 4 source points\n",
    "    src = np.float32([[180, img.shape[0]], [575, 460], \n",
    "                      [705, 460], [1150, img.shape[0]]])\n",
    "    # Define 4 destination points\n",
    "    dst = np.float32([[320, img.shape[0]], [320, 0], \n",
    "                      [960, 0], [960, img.shape[0]]])\n",
    "    # Use cv2.getPerspectiveTransform() to get M, the transform matrix\n",
    "    if inv == False:\n",
    "        M = cv2.getPerspectiveTransform(src, dst)\n",
    "    else:\n",
    "        M = cv2.getPerspectiveTransform(dst, src)\n",
    "    # Use cv2.warpPerspective() to warp image to a top-down view\n",
    "    img_size = (img.shape[1], img.shape[0])\n",
    "    warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)\n",
    "    return warped"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Image Pipeline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Define a class to receive the characteristics of each line detection\n",
    "class Line():\n",
    "    def __init__(self):\n",
    "        # Was the line detected in the last iteration?\n",
    "        self.detected = False  \n",
    "        # x values of the last n fits of the line\n",
    "        self.recent_xfitted = collections.deque(12*[0.0, 0.0, 0.0], 12)\n",
    "        # Average x values of the fitted line over the last n iterations\n",
    "        self.bestx = None\n",
    "        # Polynomial coefficients averaged over the last n iterations\n",
    "        self.best_fit = None  \n",
    "        # Polynomial coefficients for the most recent fit\n",
    "        self.current_fit = [np.array([False])]  \n",
    "        # Radius of curvature of the line in some units (meters)\n",
    "        self.radius_of_curvature = None \n",
    "        # Distance in meters of vehicle center from the line\n",
    "        self.line_base_pos = None\n",
    "        # difference in fit coefficients between last and new fits\n",
    "        self.diffs = np.array([0,0,0], dtype='float') \n",
    "        # x values for detected line pixels\n",
    "        self.allx = None\n",
    "        # y values for detected line pixels\n",
    "        self.ally = None"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# CAMERA CALIBRATION AND IMAGE UNDISTORTION\n",
    "def get_undist():\n",
    "    # Test undistortion on the image\n",
    "    img = cv2.imread('camera_cal/calibration1.jpg')\n",
    "    #img = mpimg.imread('test_images/test1.jpg')\n",
    "    # Calibrate camera and save data to pickle\n",
    "    #mtx, dist = calibrate_camera(img)\n",
    "    # Load calibration data from pickle\n",
    "    mtx, dist = pickle_load()\n",
    "    # Undistort image\n",
    "    dst = cv2.undistort(img, mtx, dist, None, mtx)\n",
    "    # Visualize undistortion\n",
    "    #plot_row2(img, dst, 'Original Image', 'Undistorted Image')\n",
    "    return mtx, dist"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# COLOR / GRADIENT THRESHOLD\n",
    "mtx, dist = get_undist()\n",
    "def threshold(image):\n",
    "    # Get calibration data\n",
    "    #mtx, dist = get_undist()\n",
    "    # Undistort image\n",
    "    undist_image = cv2.undistort(image, mtx, dist, None, mtx)\n",
    "    # Perform Sobel operations and combine thresholds\n",
    "    combine_sobel = combine_sobel_thresholds(undist_image, do_plot=False)\n",
    "    # Threshold color channel\n",
    "    color_thresh = color_channel_threshold(undist_image, thresh=(160, 255), do_plot=False)\n",
    "    # Combine color and gradient thresholds\n",
    "    combined_binary = np.zeros_like(color_thresh)\n",
    "    combined_binary[(combine_sobel == 1) | (color_thresh == 1)] = 1\n",
    "    # Plot results\n",
    "    #plot_row2(combine_sobel, color_thresh, 'Combined Sobel operations', 'S threshold', graysc=True)\n",
    "    #plot_row2(undist_image, combined_binary, 'Original Image (Undistorted)', 'Final Thresholded Image')\n",
    "    return combined_binary, undist_image"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# PERSPECTIVE TRANSFORM\n",
    "def warp(thresholded):\n",
    "    # Plot borders (for experiments)\n",
    "    #img = mpimg.imread('test_images/straight_lines1.jpg')\n",
    "    #pts = np.array([[180, img.shape[0]], [575, 460], [705, 460], [1150, img.shape[0]]], np.int32)\n",
    "    #pts = pts.reshape((-1,1,2))\n",
    "    #cv2.polylines(img, [pts], True, (0,255,255), 1)\n",
    "    #img_tr = perspective_transform(img)\n",
    "    #plot_row2(img, img_tr, 'Undistorted image', 'Warped image')\n",
    "    warped_img = perspective_transform(thresholded)\n",
    "    #plot_row2(thresholded, warped_img, 'Combined binary image', 'Warped image')\n",
    "    # Define image mask (polygon of interest)\n",
    "    imshape = warped_img.shape\n",
    "    vertices = np.array([[(200, imshape[0]), (200, 0), (imshape[1] - 200, 0), \n",
    "                      (imshape[1]-200, imshape[0])]], dtype=np.int32)\n",
    "    masked_img = region_of_interest(warped_img, vertices)\n",
    "    #plot_row2(warped_img, masked_img, 'Warped image', 'Warped image with mask')\n",
    "    return masked_img"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "import collections\n",
    "\n",
    "def find_base_pts(warped):\n",
    "    # Take a histogram of the bottom half of the masked image\n",
    "    histogram = np.sum(warped[warped.shape[0]//2:,:], axis=0)\n",
    "    #plt.plot(histogram)\n",
    "    # Find the peak of the left and right halves of the histogram\n",
    "    midpoint = np.int(histogram.shape[0]/2)\n",
    "    leftx_base = np.argmax(histogram[:midpoint])\n",
    "    rightx_base = np.argmax(histogram[midpoint:]) + midpoint\n",
    "    return midpoint, leftx_base, rightx_base\n",
    "\n",
    "# DETECT LANE LINES\n",
    "def detect_lines(warped):\n",
    "    lines_detected = False\n",
    "    \n",
    "    # Find the starting point for the left and right lines\n",
    "    midpoint, leftx_base, rightx_base = find_base_pts(warped)\n",
    "    # Create an output image to draw on and  visualize the result\n",
    "    out_img = np.dstack((warped, warped, warped))*255\n",
    "\n",
    "    # Number of sliding windows\n",
    "    nwindows = 9\n",
    "    # Height of windows: e.g. 720/9=80\n",
    "    window_height = np.int(warped.shape[0] / nwindows)\n",
    "    \n",
    "    # Identify the x and y positions of all nonzero pixels in the image\n",
    "    nonzero = warped.nonzero()\n",
    "    nonzeroy = np.array(nonzero[0])\n",
    "    nonzerox = np.array(nonzero[1])\n",
    "    \n",
    "    # Current positions to be updated for each window\n",
    "    leftx_current = leftx_base\n",
    "    rightx_current = rightx_base\n",
    "    # Set the width of the windows +/- margin\n",
    "    margin = 100\n",
    "    # Set minimum number of pixels found to recenter window\n",
    "    minpix = 50\n",
    "    # Create empty lists to receive left and right lane pixel indices\n",
    "    left_lane_inds = []\n",
    "    right_lane_inds = []\n",
    "    \n",
    "    # Sliding windows\n",
    "    if (left_line.detected == False) or (right_line.detected == False) :\n",
    "        # Step through the windows one by one\n",
    "        for window in range(nwindows):\n",
    "            # Identify window boundaries in x and y (and right and left)\n",
    "            win_y_low = warped.shape[0] - (window + 1) * window_height\n",
    "            win_y_high = warped.shape[0] - window * window_height\n",
    "            win_xleft_low = leftx_current - margin\n",
    "            win_xleft_high = leftx_current + margin\n",
    "            win_xright_low = rightx_current - margin\n",
    "            win_xright_high = rightx_current + margin\n",
    "            # Draw the windows on the visualization image\n",
    "            #cv2.rectangle(out_img, (win_xleft_low, win_y_low), (win_xleft_high, win_y_high), (0,255,0), 5) \n",
    "            #cv2.rectangle(out_img, (win_xright_low, win_y_low), (win_xright_high, win_y_high), (0,255,0), 5) \n",
    "            # Identify the nonzero pixels in x and y within the window\n",
    "            good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & \n",
    "                              (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]\n",
    "            good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & \n",
    "                               (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]\n",
    "            \n",
    "            # Append these indices to the lists\n",
    "            left_lane_inds.append(good_left_inds)\n",
    "            right_lane_inds.append(good_right_inds)\n",
    "            \n",
    "            # If you found > minpix pixels, recenter next window on their mean position\n",
    "            if len(good_left_inds) > minpix:\n",
    "                leftx_current = np.int(np.mean(nonzerox[good_left_inds]))\n",
    "            if len(good_right_inds) > minpix:        \n",
    "                rightx_current = np.int(np.mean(nonzerox[good_right_inds]))\n",
    "                \n",
    "        # Concatenate the arrays of indices\n",
    "        left_lane_inds = np.concatenate(left_lane_inds)\n",
    "        right_lane_inds = np.concatenate(right_lane_inds)\n",
    "        left_line.detected = True\n",
    "        right_line.detected = True\n",
    "    else:\n",
    "        left_lane_inds = ((nonzerox > (left_line.current_fit[0] * (nonzeroy**2) + \n",
    "                                       left_line.current_fit[1] * nonzeroy + \n",
    "                                       left_line.current_fit[2] - margin)) & \n",
    "                          (nonzerox < (left_line.current_fit[0] * (nonzeroy**2) + \n",
    "                                       left_line.current_fit[1] * nonzeroy + \n",
    "                                       left_line.current_fit[2] + margin))) \n",
    "        right_lane_inds = ((nonzerox > (right_line.current_fit[0] * (nonzeroy**2) + \n",
    "                                        right_line.current_fit[1] * nonzeroy + \n",
    "                                        right_line.current_fit[2] - margin)) & \n",
    "                           (nonzerox < (right_line.current_fit[0] * (nonzeroy**2) + \n",
    "                                        right_line.current_fit[1] * nonzeroy + \n",
    "                                        right_line.current_fit[2] + margin)))\n",
    "    # Extract left and right line pixel positions\n",
    "    leftx = nonzerox[left_lane_inds]\n",
    "    lefty = nonzeroy[left_lane_inds]\n",
    "    rightx = nonzerox[right_lane_inds]\n",
    "    righty = nonzeroy[right_lane_inds]\n",
    "\n",
    "    # Here we need to save successful fit of lines to prevent case with empty x, y\n",
    "    if (len(leftx) < 1500):\n",
    "        leftx = left_line.allx\n",
    "        lefty = left_line.ally\n",
    "        left_line.detected = False\n",
    "    else:\n",
    "        left_line.allx = leftx\n",
    "        left_line.ally = lefty\n",
    "    if (len(rightx) < 1500):\n",
    "        rightx = right_line.allx\n",
    "        righty = right_line.ally\n",
    "        right_line.detected = False\n",
    "    else:\n",
    "        right_line.allx = rightx\n",
    "        right_line.ally = righty\n",
    "    \n",
    "    # Fit a second order polynomial to each\n",
    "    right_fit = np.polyfit(righty, rightx, 2)\n",
    "    left_fit = np.polyfit(lefty, leftx, 2)\n",
    "    \n",
    "    # Sanity check:\n",
    "    # INIT\n",
    "    if (left_line.current_fit[0] == False):\n",
    "        left_line.current_fit = left_fit\n",
    "        right_line.current_fit = right_fit\n",
    "    \n",
    "    if (abs(left_line.current_fit[1] - left_fit[1]) > 0.18):\n",
    "        #print('FOR_UPD_DIFF_LEFT_C2 = {}'.format(abs(left_line.current_fit[1] - left_fit[1])))\n",
    "        #print('FOR_UPD_DIFF_LEFT_C3 = {}'.format(abs(left_line.current_fit[2] - left_fit[2])))\n",
    "        left_line.current_fit = left_line.best_fit\n",
    "        #print('UPD_LEFT left_fit_1 = {} | right_fit_1 = {}'.format(left_line.current_fit[0], right_line.current_fit[0]))\n",
    "        #print('UPD_LEFT left_fit_2 = {} | right_fit_2 = {}'.format(left_line.current_fit[1], right_line.current_fit[1]))\n",
    "        #print('UPD_LEFT left_fit_3 = {} | right_fit_3 = {}'.format(left_line.current_fit[2], right_line.current_fit[2]))\n",
    "        left_line.detected = False\n",
    "    else:\n",
    "        #print('NORMAL_DIFF_LEFT_C2 = {}'.format(abs(left_line.current_fit[1] - left_fit[1])))\n",
    "        #print('NORMAL_DIFF_LEFT_C3 = {}'.format(abs(left_line.current_fit[2] - left_fit[2])))\n",
    "        left_line.current_fit = left_fit\n",
    "        #print('NORMAL_LEFT left_fit_1 = {} | right_fit_1 = {}'.format(left_line.current_fit[0], right_line.current_fit[0]))\n",
    "        #print('NORMAL_LEFT left_fit_2 = {} | right_fit_2 = {}'.format(left_line.current_fit[1], right_line.current_fit[1]))\n",
    "        #print('NORMAL_LEFT left_fit_3 = {} | right_fit_3 = {}'.format(left_line.current_fit[2], right_line.current_fit[2]))\n",
    "        left_line.recent_xfitted.pop()\n",
    "        left_line.recent_xfitted.appendleft(left_line.current_fit)\n",
    "        avg = np.array([0,0,0], dtype='float')\n",
    "        for element in left_line.recent_xfitted:\n",
    "            avg = avg + element\n",
    "        left_line.best_fit = avg / (len(left_line.recent_xfitted))\n",
    "        \n",
    "    if (abs(right_line.current_fit[1] - right_fit[1]) > 0.18):\n",
    "        #print('FOR_UPD_DIFF_RIGHT_C2 = {}'.format(abs(right_line.current_fit[1] - right_fit[1])))\n",
    "        #print('FOR_UPD_DIFF_RIGHT_C3 = {}'.format(abs(right_line.current_fit[2] - right_fit[2])))\n",
    "        right_line.current_fit = right_line.best_fit\n",
    "        #print('UPD_RIGHT left_fit_1 = {} | right_fit_1 = {}'.format(left_line.current_fit[0], right_line.current_fit[0]))\n",
    "        #print('UPD_RIGHT left_fit_2 = {} | right_fit_2 = {}'.format(left_line.current_fit[1], right_line.current_fit[1]))\n",
    "        #print('UPD_RIGHT left_fit_3 = {} | right_fit_3 = {}'.format(left_line.current_fit[2], right_line.current_fit[2]))\n",
    "        right_line.detected = False\n",
    "    else:\n",
    "        #print('NORMAL_DIFF_RIGHT_C2 = {}'.format(abs(right_line.current_fit[1] - right_fit[1])))\n",
    "        #print('NORMAL_DIFF_RIGHT_C3 = {}'.format(abs(right_line.current_fit[2] - right_fit[2])))\n",
    "        right_line.current_fit = right_fit\n",
    "        #print('NORMAL_RIGHT left_fit_1 = {} | right_fit_1 = {}'.format(left_line.current_fit[0], right_line.current_fit[0]))\n",
    "        #print('NORMAL_RIGHT left_fit_2 = {} | right_fit_2 = {}'.format(left_line.current_fit[1], right_line.current_fit[1]))\n",
    "        #print('NORMAL_RIGHT left_fit_3 = {} | right_fit_3 = {}'.format(left_line.current_fit[2], right_line.current_fit[2]))\n",
    "        right_line.recent_xfitted.pop()\n",
    "        right_line.recent_xfitted.appendleft(right_line.current_fit)\n",
    "        avg = np.array([0,0,0], dtype='float')\n",
    "        for element in right_line.recent_xfitted:\n",
    "            avg = avg + element\n",
    "        right_line.best_fit = avg / (len(right_line.recent_xfitted))\n",
    "        \n",
    "    if (abs(right_line.current_fit[1] - right_fit[1]) > 0.38 and\n",
    "        abs(left_line.current_fit[1] - left_fit[1]) < 0.1):\n",
    "        right_line.current_fit[0] = left_line.current_fit[0]\n",
    "        right_line.current_fit[1] = left_line.current_fit[1]\n",
    "        right_line.current_fit[2] = left_line.current_fit[2] + 600\n",
    "        right_line.recent_xfitted.pop()\n",
    "        right_line.recent_xfitted.appendleft(right_line.current_fit)\n",
    "        avg = np.array([0,0,0], dtype='float')\n",
    "        for element in right_line.recent_xfitted:\n",
    "            avg = avg + element\n",
    "        right_line.best_fit = avg / (len(right_line.recent_xfitted))\n",
    "        \n",
    "    if (abs(left_line.current_fit[1] - left_fit[1]) > 0.38 and\n",
    "        abs(right_line.current_fit[1] - right_fit[1]) < 0.1):\n",
    "        left_line.current_fit = left_fit\n",
    "        left_line.recent_xfitted.pop()\n",
    "        left_line.recent_xfitted.appendleft(left_line.current_fit)\n",
    "        avg = np.array([0,0,0], dtype='float')\n",
    "        for element in left_line.recent_xfitted:\n",
    "            avg = avg + element\n",
    "        left_line.best_fit = avg / (len(left_line.recent_xfitted))\n",
    "    \n",
    "    # Generate x and y values for plotting\n",
    "    ploty = np.linspace(0, warped.shape[0] - 1, warped.shape[0] )\n",
    "    left_fitx = (left_line.current_fit[0] * ploty**2 + \n",
    "                 left_line.current_fit[1] * ploty + \n",
    "                 left_line.current_fit[2])\n",
    "    right_fitx = (right_line.current_fit[0] * ploty**2 + \n",
    "                  right_line.current_fit[1] * ploty + \n",
    "                  right_line.current_fit[2])\n",
    "    out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]\n",
    "    out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]\n",
    "    #fig = plt.figure(figsize=(18, 6))\n",
    "    #plt.imshow(out_img)\n",
    "    #plt.plot(left_fitx, ploty, color='yellow')\n",
    "    #plt.plot(right_fitx, ploty, color='yellow')\n",
    "    #plt.xlim(0, 1280)\n",
    "    #plt.ylim(720, 0)\n",
    "    return out_img, ploty, leftx, lefty, rightx, righty, left_fit, right_fit, left_fitx, right_fitx"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# VEHICLE OFFSET FROM LANE CENTER\n",
    "def calc_offset(xm_per_pix, ym_per_pix, y_eval, left_fit_cr, right_fit_cr):\n",
    "    # Calculate x bottom position for y for left lane\n",
    "    left_lane_bottom = (left_fit_cr[0] * (y_eval * ym_per_pix) ** 2 + \n",
    "                        left_fit_cr[1] * (y_eval * ym_per_pix) + left_fit_cr[2])\n",
    "    # Calculate x bottom position for y for right lane\n",
    "    right_lane_bottom = (right_fit_cr[0] * (y_eval * ym_per_pix) ** 2 + \n",
    "                         right_fit_cr[1] * (y_eval * ym_per_pix) + right_fit_cr[2])\n",
    "    # Calculate the mid point of the lane\n",
    "    lane_midpoint = float(right_lane_bottom + left_lane_bottom) / 2\n",
    "    # Calculate the image center in meters from left edge of the image\n",
    "    image_mid_point_in_meter = 1280/2 * xm_per_pix\n",
    "    # Positive value indicates vehicle on the right side of lane center, else on the left.\n",
    "    lane_deviation = (image_mid_point_in_meter - lane_midpoint)\n",
    "    #print ('left_lane_bottom = {:.3f}'.format(left_lane_bottom))\n",
    "    #print ('right_lane_bottom = {:.3f}'.format(right_lane_bottom))\n",
    "    #print ('lane_midpoint = {:.3f}'.format(lane_midpoint))\n",
    "    #print ('image_mid_point_in_meter = {:.3f}'.format(image_mid_point_in_meter))\n",
    "    #print ('lane_deviation = {:.3f}'.format(lane_deviation))\n",
    "    return lane_deviation\n",
    "\n",
    "# MEASURE RADIUS OF CURVATURE AND VEHICLE OFFSET\n",
    "def rad_and_offset(ploty, leftx, lefty, rightx, righty, left_fit, right_fit):\n",
    "    # Plot up the data\n",
    "    #mark_size = 0.1\n",
    "    #plt.plot(leftx, lefty, 'o', color='red', markersize=mark_size)\n",
    "    #plt.plot(rightx, righty, 'o', color='blue', markersize=mark_size)\n",
    "    #plt.xlim(0, 1280)\n",
    "    #plt.ylim(0, 720)\n",
    "    #plt.plot(left_fitx, ploty, color='green', linewidth=3)\n",
    "    #plt.plot(right_fitx, ploty, color='green', linewidth=3)\n",
    "    #plt.gca().invert_yaxis()\n",
    "\n",
    "    # Define y-value where we want radius of curvature\n",
    "    # I'll choose the maximum y-value, corresponding to the bottom of the image\n",
    "    y_eval = np.max(ploty)\n",
    "    left_curverad = (((1 + (2 * left_fit[0] * y_eval + left_fit[1])**2)**1.5) / \n",
    "                     np.absolute(2 * left_fit[0]))\n",
    "    right_curverad = (((1 + (2 * right_fit[0] * y_eval + right_fit[1])**2)**1.5) / \n",
    "                      np.absolute(2 * right_fit[0]))\n",
    "    curverad_avg = (left_curverad + right_curverad)/2\n",
    "    #print('Left: {}, Right: {}, AVG: {}'.format(left_curverad, right_curverad, curverad_avg))\n",
    "\n",
    "    # Define conversions in x and y from pixels space to meters\n",
    "    ym_per_pix = 30/720   # meters per pixel in y dimension\n",
    "    xm_per_pix = 3.7/700  # meters per pixel in x dimension\n",
    "    # Fit a second order polynomial to each\n",
    "    left_fit_cr = np.polyfit(lefty * ym_per_pix, leftx * xm_per_pix, 2)\n",
    "    right_fit_cr = np.polyfit(righty * ym_per_pix, rightx * xm_per_pix, 2)\n",
    "\n",
    "    # Calculate the new radius of curvature in meters\n",
    "    left_curverad = ((1 + (2 * left_fit_cr[0] * y_eval * ym_per_pix + \n",
    "                           left_fit_cr[1])**2)**1.5) / np.absolute(2 * left_fit_cr[0])\n",
    "    right_curverad = ((1 + (2 * right_fit_cr[0] * y_eval * ym_per_pix + \n",
    "                            right_fit_cr[1])**2)**1.5) / np.absolute(2 * right_fit_cr[0])\n",
    "    curverad_avg = (left_curverad + right_curverad) / 2\n",
    "    # Vehicle offset\n",
    "    lane_deviation = calc_offset(xm_per_pix, ym_per_pix, y_eval, left_fit_cr, right_fit_cr)\n",
    "    \n",
    "    #print('Left: {} m, Right: {} m, AVG: {} m'.format(left_curverad, right_curverad, curverad_avg))\n",
    "    #print ('lane_deviation = {:.3f}'.format(lane_deviation))\n",
    "    return left_curverad, right_curverad, lane_deviation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# DRAWING\n",
    "def draw_res(warped, undistorted, out_img, ploty, left_fitx, right_fitx, left_curverad, right_curverad, lane_deviation):\n",
    "    # Create an image to draw the lines on\n",
    "    warp_zero = np.zeros_like(warped).astype(np.uint8)\n",
    "    color_warp = np.dstack((warped, warped, warped))\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, ploty]))])\n",
    "    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])\n",
    "    pts = np.hstack((pts_left, pts_right))\n",
    "\n",
    "    # Draw the lane onto the warped blank image\n",
    "    cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))\n",
    "    # Warp the blank back to original image space using inverse perspective matrix (Minv)\n",
    "    newwarp = perspective_transform(color_warp, inv=True)\n",
    "    # Combine the result with the original image\n",
    "    result = cv2.addWeighted(undistorted, 1, newwarp, 0.3, 0)\n",
    "\n",
    "    x_offset = result.shape[1] - 320 - 30\n",
    "    y_offset = 30\n",
    "    thumb = cv2.resize(out_img, (320, 200), interpolation = cv2.INTER_CUBIC)\n",
    "    result[y_offset:y_offset + thumb.shape[0], x_offset:x_offset + thumb.shape[1]] = thumb\n",
    "\n",
    "    font = cv2.FONT_HERSHEY_SIMPLEX\n",
    "    curv_l_label = 'Radius of Curvature (Left line): {:.0f} m.'.format(left_curverad)\n",
    "    curv_r_label = 'Radius of Curvature (Right line): {:.0f} m.'.format(right_curverad)\n",
    "    deviation_label = 'Vehicle Deviation: {:.3f} m.'.format(lane_deviation)\n",
    "\n",
    "    cv2.putText(result, curv_l_label, (30, 60), font, 1, (255,255,255), 2)\n",
    "    cv2.putText(result, curv_r_label, (30, 110), font, 1, (255,255,255), 2)\n",
    "    cv2.putText(result, deviation_label, (30, 160), font, 1, (255,255,255), 2)\n",
    "\n",
    "    #plot_row2(undist_image, result, 'Original Frame (Undistorted)', 'Processed Frame')\n",
    "    #fig = plt.figure(figsize=(20, 8))\n",
    "    #plt.imshow(result)\n",
    "    return result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "def image_pipeline(image):\n",
    "    # Threshold image\n",
    "    thresholded, undistorted = threshold(image)\n",
    "    # Warp image\n",
    "    warped = warp(thresholded)\n",
    "    # Detect lane lines\n",
    "    out_img, ploty, leftx, lefty, rightx, righty, left_fit, right_fit, left_fitx, right_fitx = detect_lines(warped)\n",
    "    # Radius of curvature for left and right lines\n",
    "    left_curverad, right_curverad, lane_deviation = rad_and_offset(ploty, leftx, lefty, rightx, \n",
    "                                                                   righty, left_fit, right_fit)\n",
    "    # Draw output\n",
    "    proc_img = draw_res(warped, undistorted, out_img, ploty, left_fitx, right_fitx, \n",
    "                        left_curverad, right_curverad, lane_deviation)\n",
    "    return proc_img"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# MONITOR (FOR TESTING IMAGE PIPELINE)\n",
    "# Load original image from camera\n",
    "left_line = Line()\n",
    "right_line = Line()\n",
    "image = mpimg.imread('test_images/test1.jpg')\n",
    "proc_img = image_pipeline(image)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# MONITOR (FOR TESTING VIDEO PIPELINE)\n",
    "# Import everything needed to edit/save/watch video clips\n",
    "from moviepy.editor import VideoFileClip\n",
    "from IPython.display import HTML\n",
    "left_line = Line()\n",
    "right_line = Line()\n",
    "output_video = 'project_video_processed.mp4'\n",
    "clip1 = VideoFileClip(\"project_video.mp4\")\n",
    "video_clip = clip1.fl_image(image_pipeline)\n",
    "%time video_clip.write_videofile(output_video, audio=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "HTML(\"\"\"\n",
    "<video width=\"960\" height=\"540\" controls>\n",
    "  <source src=\"{0}\">\n",
    "</video>\n",
    "\"\"\".format(output_video))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "deletable": true,
    "editable": 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"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}