Vehicle-Detection.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Vehicle Detection + Advanced Lane Finding\n",
    "\n",
    "The goals / steps of this project are the following:\n",
    "\n",
    "* Perform a Histogram of Oriented Gradients (HOG) feature extraction on a labeled training set of images and train a classifier Linear SVM classifier.\n",
    "* Apply a color transform and append binned color features, as well as histograms of color, to HOG feature vector.\n",
    "* Normalize features and randomize a selection for training and testing.\n",
    "* Implement a sliding-window technique and use trained classifier to search for vehicles in images.\n",
    "* Run pipeline on a video stream (start with the test_video.mp4 and later implement on full project_video.mp4) and create a heat map of recurring detections frame by frame to reject outliers and follow detected vehicles.\n",
    "* Estimate a bounding box for vehicles detected.\n",
    "* Add Advanced Lane Finding pipeline.\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",
    "import pickle\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",
    "    # 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",
    "    # List of calibration images\n",
    "    images = glob.glob('camera_cal/calibration*.jpg')\n",
    "    print('Num of calibration images: {0}'.format(len(images)))\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": [
    "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",
    "    # 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",
    "    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",
    "    # 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",
    "    if do_plot:\n",
    "        plot_row2(image, s_binary, 'Original Image (Undistorted)', 'HLS(S-channel) threshold')\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,
    "deletable": true,
    "editable": 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",
    "    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",
    "    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",
    "    warped_img = perspective_transform(thresholded)\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",
    "    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",
    "    # 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",
    "    # 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",
    "    # 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",
    "    # 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",
    "    # 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",
    "    # 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",
    "            # 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",
    "            # Append these indices to the lists\n",
    "            left_lane_inds.append(good_left_inds)\n",
    "            right_lane_inds.append(good_right_inds)\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",
    "        # 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",
    "    # 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",
    "    # 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",
    "    # Initial iteration\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",
    "        left_line.current_fit = left_line.best_fit\n",
    "        left_line.detected = False\n",
    "    else:\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",
    "    if (abs(right_line.current_fit[1] - right_fit[1]) > 0.18):\n",
    "        right_line.current_fit = right_line.best_fit\n",
    "        right_line.detected = False\n",
    "    else:\n",
    "        right_line.current_fit = right_fit\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",
    "\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",
    "    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",
    "    # 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",
    "\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",
    "    return left_curverad, right_curverad, lane_deviation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "def draw_labeled_bboxes(img, labels):\n",
    "    # Iterate through all detected cars\n",
    "    for car_number in range(1, labels[1]+1):\n",
    "        # Find pixels with each car_number label value\n",
    "        nonzero = (labels[0] == car_number).nonzero()\n",
    "        # Identify x and y values of those pixels\n",
    "        nonzeroy = np.array(nonzero[0])\n",
    "        nonzerox = np.array(nonzero[1])\n",
    "        # Define a bounding box based on min/max x and y\n",
    "        bbox = ((np.min(nonzerox), np.min(nonzeroy)), (np.max(nonzerox), np.max(nonzeroy)))\n",
    "        # Draw the box on the image\n",
    "        cv2.rectangle(img, bbox[0], bbox[1], (0,0,255), 6)\n",
    "    # Return the image\n",
    "    return img\n",
    "\n",
    "# DRAWING\n",
    "def draw_res(warped, undistorted, out_img, ploty, left_fitx, \n",
    "             right_fitx, left_curverad, right_curverad, lane_deviation, labels, heatmap):\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_offset1 = result.shape[1] - 320 - 30\n",
    "    y_offset = 65\n",
    "    thumb = cv2.resize(out_img, (320, 200), interpolation = cv2.INTER_CUBIC)\n",
    "    result[y_offset:y_offset + thumb.shape[0], x_offset1:x_offset1 + thumb.shape[1]] = thumb\n",
    "    \n",
    "    # Resize the heatmap image\n",
    "    resized_heatmap = 255*cv2.resize(heatmap, (320, 200), interpolation=cv2.INTER_AREA)\n",
    "    # Compose the 3 channel Heatmap\n",
    "    thumb = cv2.merge([resized_heatmap, resized_heatmap, resized_heatmap])\n",
    "    # Add Heatmap to the Image\n",
    "    x_offset2 = result.shape[1] - 320*2 - 30*2\n",
    "    result[y_offset:y_offset + thumb.shape[0], x_offset2:x_offset2 + thumb.shape[1]] = thumb\n",
    "\n",
    "    font = cv2.FONT_HERSHEY_SIMPLEX\n",
    "    curv_l_label = 'Left line Curvature: {:.0f} m.'.format(left_curverad)\n",
    "    curv_r_label = 'Right line Curvature: {:.0f} m.'.format(right_curverad)\n",
    "    deviation_label = 'Vehicle Deviation: {:.3f} m.'.format(lane_deviation)\n",
    "\n",
    "    cv2.putText(result, curv_l_label, (30, 50), font, 1, (255,255,255), 2)\n",
    "    cv2.putText(result, curv_r_label, (30, 100), font, 1, (255,255,255), 2)\n",
    "    cv2.putText(result, deviation_label, (30, 150), font, 1, (255,255,255), 2)\n",
    "    cv2.putText(result, 'Vehicles Heatmap', (x_offset2+20, 50), font, 1, (255,255,255), 2)\n",
    "    cv2.putText(result, 'Lane Lines', (x_offset1+70, 50), font, 1, (255,255,255), 2)\n",
    "    result = draw_labeled_bboxes(np.copy(result), labels)\n",
    "\n",
    "    #plot_row2(undistorted, result, 'Original Frame (Undistorted)', 'Processed Frame')\n",
    "    #fig = plt.figure(figsize=(20, 8))\n",
    "    #plt.imshow(result)\n",
    "    return result"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "deletable": true,
    "editable": true
   },
   "source": [
    "## Vehicle Detection Pipeline\n",
    "\n",
    "### Helpers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "import matplotlib.image as mpimg\n",
    "import numpy as np\n",
    "import cv2\n",
    "from skimage.feature import hog\n",
    "\n",
    "# For universal plotting of results\n",
    "def plot_row4(img1, img2, img3, img4, label_1, label_2, label_3, label_4, graysc=True):\n",
    "    # Plot the result (1 row with 4 images)\n",
    "    f, (ax1, ax2, ax3, ax4) = plt.subplots(1, 4, figsize=(12, 4))\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=14)\n",
    "    ax2.imshow(img2, cmap='gray')\n",
    "    ax2.set_title(label_2, fontsize=14)\n",
    "    ax3.imshow(img3, cmap='gray')\n",
    "    ax3.set_title(label_3, fontsize=14)\n",
    "    ax4.imshow(img4, cmap='gray')\n",
    "    ax4.set_title(label_4, fontsize=14)\n",
    "    plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)\n",
    "\n",
    "    \n",
    "def convert_color(img, conv='RGB2YCrCb'):\n",
    "    if conv == 'RGB2YCrCb':\n",
    "        return cv2.cvtColor(img, cv2.COLOR_RGB2YCrCb)\n",
    "    if conv == 'BGR2YCrCb':\n",
    "        return cv2.cvtColor(img, cv2.COLOR_BGR2YCrCb)\n",
    "    if conv == 'RGB2LUV':\n",
    "        return cv2.cvtColor(img, cv2.COLOR_RGB2LUV)\n",
    "\n",
    "# Define a function to return HOG features and visualization\n",
    "def get_hog_features(img, orient, pix_per_cell, cell_per_block, \n",
    "                     vis=False, feature_vec=True):\n",
    "    # Call with two outputs if vis==True\n",
    "    if vis == True:\n",
    "        features, hog_image = hog(img, orientations=orient, \n",
    "                                  pixels_per_cell=(pix_per_cell, pix_per_cell),\n",
    "                                  cells_per_block=(cell_per_block, cell_per_block), \n",
    "                                  transform_sqrt=False, \n",
    "                                  visualise=vis, feature_vector=feature_vec)\n",
    "        return features, hog_image\n",
    "    # Otherwise call with one output\n",
    "    else:      \n",
    "        features = hog(img, orientations=orient, \n",
    "                       pixels_per_cell=(pix_per_cell, pix_per_cell),\n",
    "                       cells_per_block=(cell_per_block, cell_per_block), \n",
    "                       transform_sqrt=False, \n",
    "                       visualise=vis, feature_vector=feature_vec)\n",
    "        return features\n",
    "\n",
    "# Define a function to compute binned color features  \n",
    "def bin_spatial(img, size=(32, 32)):\n",
    "    color1 = cv2.resize(img[:,:,0], size).ravel()\n",
    "    color2 = cv2.resize(img[:,:,1], size).ravel()\n",
    "    color3 = cv2.resize(img[:,:,2], size).ravel()\n",
    "    return np.hstack((color1, color2, color3))\n",
    "\n",
    "# Define a function to compute color histogram features \n",
    "# NEED TO CHANGE bins_range if reading .png files with mpimg!\n",
    "def color_hist(img, nbins=32):    #bins_range=(0, 256)\n",
    "    # Compute the histogram of the color channels separately\n",
    "    channel1_hist = np.histogram(img[:,:,0], bins=nbins)\n",
    "    channel2_hist = np.histogram(img[:,:,1], bins=nbins)\n",
    "    channel3_hist = np.histogram(img[:,:,2], bins=nbins)\n",
    "    # Concatenate the histograms into a single feature vector\n",
    "    hist_features = np.concatenate((channel1_hist[0], channel2_hist[0], channel3_hist[0]))\n",
    "    # Return the individual histograms, bin_centers and feature vector\n",
    "    return hist_features\n",
    "\n",
    "\n",
    "# Define a function to extract features from a single image window\n",
    "# This function is very similar to extract_features()\n",
    "# just for a single image rather than list of images\n",
    "def single_img_features(img, color_space='RGB', spatial_size=(32, 32),\n",
    "                        hist_bins=32, orient=9, \n",
    "                        pix_per_cell=8, cell_per_block=2, hog_channel=0,\n",
    "                        spatial_feat=True, hist_feat=True, hog_feat=True):\n",
    "    #1) Define an empty list to receive features\n",
    "    img_features = []\n",
    "    #2) Apply color conversion if other than 'RGB'\n",
    "    if color_space != 'RGB':\n",
    "        if color_space == 'HSV':\n",
    "            feature_image = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)\n",
    "        elif color_space == 'LUV':\n",
    "            feature_image = cv2.cvtColor(img, cv2.COLOR_RGB2LUV)\n",
    "        elif color_space == 'HLS':\n",
    "            feature_image = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)\n",
    "        elif color_space == 'YUV':\n",
    "            feature_image = cv2.cvtColor(img, cv2.COLOR_RGB2YUV)\n",
    "        elif color_space == 'YCrCb':\n",
    "            feature_image = cv2.cvtColor(img, cv2.COLOR_RGB2YCrCb)\n",
    "    else: feature_image = np.copy(img)      \n",
    "    #3) Compute spatial features if flag is set\n",
    "    if spatial_feat == True:\n",
    "        spatial_features = bin_spatial(feature_image, size=spatial_size)\n",
    "        #4) Append features to list\n",
    "        img_features.append(spatial_features)\n",
    "    #5) Compute histogram features if flag is set\n",
    "    if hist_feat == True:\n",
    "        hist_features = color_hist(feature_image, nbins=hist_bins)\n",
    "        #6) Append features to list\n",
    "        img_features.append(hist_features)\n",
    "    #7) Compute HOG features if flag is set\n",
    "    if hog_feat == True:\n",
    "        if hog_channel == 'ALL':\n",
    "            hog_features = []\n",
    "            for channel in range(feature_image.shape[2]):\n",
    "                hog_features.extend(get_hog_features(feature_image[:,:,channel], \n",
    "                                    orient, pix_per_cell, cell_per_block, \n",
    "                                    vis=False, feature_vec=True))      \n",
    "        else:\n",
    "            hog_features = get_hog_features(feature_image[:,:,hog_channel], orient, \n",
    "                        pix_per_cell, cell_per_block, vis=False, feature_vec=True)\n",
    "        # For plotting\n",
    "        pics = []\n",
    "        for channel in range(feature_image.shape[2]):\n",
    "            feat, pic = get_hog_features(feature_image[:,:,channel], orient, pix_per_cell, \n",
    "                                     cell_per_block, vis=True, feature_vec=True)\n",
    "            pics.append(pic)\n",
    "        plot_row4(feature_image, pics[0], pics[1], pics[2], 'YCrCb Image', 'HOG channel 0', \n",
    "                  'HOG channel 1', 'HOG channel 2', graysc=True)\n",
    "        #8) Append features to list\n",
    "        img_features.append(hog_features)\n",
    "    #9) Return concatenated array of features\n",
    "    return np.concatenate(img_features)\n",
    "\n",
    "\n",
    "# Define a function to extract features from a list of images\n",
    "# Have this function call bin_spatial() and color_hist()\n",
    "def extract_features(imgs, color_space='RGB', spatial_size=(32, 32),\n",
    "                     hist_bins=32, orient=9, \n",
    "                     pix_per_cell=8, cell_per_block=2, hog_channel=0,\n",
    "                     spatial_feat=True, hist_feat=True, hog_feat=True):\n",
    "    # Create a list to append feature vectors to\n",
    "    features = []\n",
    "    # Iterate through the list of images\n",
    "    for file in imgs:\n",
    "        file_features = []\n",
    "        # Read in each one by one\n",
    "        image = mpimg.imread(file)\n",
    "        # apply color conversion if other than 'RGB'\n",
    "        if color_space != 'RGB':\n",
    "            if color_space == 'HSV':\n",
    "                feature_image = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)\n",
    "            elif color_space == 'LUV':\n",
    "                feature_image = cv2.cvtColor(image, cv2.COLOR_RGB2LUV)\n",
    "            elif color_space == 'HLS':\n",
    "                feature_image = cv2.cvtColor(image, cv2.COLOR_RGB2HLS)\n",
    "            elif color_space == 'YUV':\n",
    "                feature_image = cv2.cvtColor(image, cv2.COLOR_RGB2YUV)\n",
    "            elif color_space == 'YCrCb':\n",
    "                feature_image = cv2.cvtColor(image, cv2.COLOR_RGB2YCrCb)\n",
    "        else: feature_image = np.copy(image)      \n",
    "\n",
    "        if spatial_feat == True:\n",
    "            spatial_features = bin_spatial(feature_image, size=spatial_size)\n",
    "            file_features.append(spatial_features)\n",
    "        if hist_feat == True:\n",
    "            # Apply color_hist()\n",
    "            hist_features = color_hist(feature_image, nbins=hist_bins)\n",
    "            file_features.append(hist_features)\n",
    "        if hog_feat == True:\n",
    "        # Call get_hog_features() with vis=False, feature_vec=True\n",
    "            if hog_channel == 'ALL':\n",
    "                hog_features = []\n",
    "                for channel in range(feature_image.shape[2]):\n",
    "                    hog_features.append(get_hog_features(feature_image[:,:,channel], \n",
    "                                        orient, pix_per_cell, cell_per_block, \n",
    "                                        vis=False, feature_vec=True))\n",
    "                hog_features = np.ravel(hog_features)        \n",
    "            else:\n",
    "                hog_features = get_hog_features(feature_image[:,:,hog_channel], orient, \n",
    "                            pix_per_cell, cell_per_block, vis=True, feature_vec=True)\n",
    "            # Append the new feature vector to the features list\n",
    "            file_features.append(hog_features)\n",
    "            \n",
    "        features.append(np.concatenate(file_features))\n",
    "    # Return list of feature vectors\n",
    "    return features"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "import matplotlib.image as mpimg\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "import cv2\n",
    "import glob\n",
    "import time\n",
    "import pickle\n",
    "from sklearn.svm import LinearSVC\n",
    "from sklearn.preprocessing import StandardScaler\n",
    "from skimage.feature import hog\n",
    "from sklearn.model_selection import train_test_split\n",
    "from scipy.ndimage.measurements import label\n",
    "%matplotlib inline\n",
    "\n",
    "# CLASSIFIER's PARAMETERS\n",
    "print('Loading dataset...')\n",
    "# Read in cars and notcars\n",
    "cars = glob.glob('dataset/vehicles/**/*.png', recursive=True)\n",
    "notcars = glob.glob('dataset/non-vehicles/**/*.png', recursive=True)\n",
    "# Stat\n",
    "print('CARS: {}'.format(len(cars)))\n",
    "print('NOTCARS: {}'.format(len(notcars)))\n",
    "# Tweak these parameters\n",
    "color_space    = 'YCrCb'    # Can be RGB, HSV, LUV, HLS, YUV, YCrCb\n",
    "orient         = 9          # HOG orientations\n",
    "pix_per_cell   = 8          # HOG pixels per cell\n",
    "cell_per_block = 2          # HOG cells per block\n",
    "hog_channel    = 'ALL'      # Can be 0, 1, 2, or \"ALL\"\n",
    "spatial_size   = (32, 32)   # Spatial binning dimensions\n",
    "hist_bins      = 32         # Number of histogram bins\n",
    "spatial_feat   = True       # Spatial features on or off\n",
    "hist_feat      = True       # Histogram features on or off\n",
    "hog_feat       = True       # HOG features on or off\n",
    "\n",
    "car = mpimg.imread(cars[10])\n",
    "notcar = mpimg.imread(notcars[10])\n",
    "#plot_row2(car, notcar, 'Car', 'Not Car')\n",
    "car_features = single_img_features(car, color_space=color_space,\n",
    "                                   spatial_size=spatial_size, hist_bins=hist_bins, \n",
    "                                   orient=orient, pix_per_cell=pix_per_cell, \n",
    "                                   cell_per_block=cell_per_block, \n",
    "                                   hog_channel=hog_channel, spatial_feat=spatial_feat, \n",
    "                                   hist_feat=hist_feat, hog_feat=hog_feat)\n",
    "print('car_features: DONE')\n",
    "notcar_features = single_img_features(notcar, color_space=color_space,\n",
    "                                      spatial_size=spatial_size, hist_bins=hist_bins, \n",
    "                                      orient=orient, pix_per_cell=pix_per_cell, \n",
    "                                      cell_per_block=cell_per_block, \n",
    "                                      hog_channel=hog_channel, spatial_feat=spatial_feat, \n",
    "                                      hist_feat=hist_feat, hog_feat=hog_feat)\n",
    "print('notcar_features: DONE')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# CLASSIFIER TRAINING PIPELINE\n",
    "car_features = extract_features(cars, color_space=color_space, \n",
    "                                spatial_size=spatial_size, hist_bins=hist_bins, \n",
    "                                orient=orient, pix_per_cell=pix_per_cell, \n",
    "                                cell_per_block=cell_per_block, \n",
    "                                hog_channel=hog_channel, spatial_feat=spatial_feat, \n",
    "                                hist_feat=hist_feat, hog_feat=hog_feat)\n",
    "print('car_features: DONE')\n",
    "notcar_features = extract_features(notcars, color_space=color_space, \n",
    "                                   spatial_size=spatial_size, hist_bins=hist_bins, \n",
    "                                   orient=orient, pix_per_cell=pix_per_cell, \n",
    "                                   cell_per_block=cell_per_block, \n",
    "                                   hog_channel=hog_channel, spatial_feat=spatial_feat, \n",
    "                                   hist_feat=hist_feat, hog_feat=hog_feat)\n",
    "print('notcar_features: DONE')\n",
    "X = np.vstack((car_features, notcar_features)).astype(np.float64)                        \n",
    "# Fit a per-column scaler\n",
    "X_scaler = StandardScaler().fit(X)\n",
    "# Apply the scaler to X\n",
    "scaled_X = X_scaler.transform(X)\n",
    "\n",
    "# Define the labels vector\n",
    "y = np.hstack((np.ones(len(car_features)), np.zeros(len(notcar_features))))\n",
    "\n",
    "# Split up data into randomized training and test sets\n",
    "rand_state = np.random.randint(0, 100)\n",
    "X_train, X_test, y_train, y_test = train_test_split(\n",
    "    scaled_X, y, test_size=0.2, random_state=rand_state)\n",
    "\n",
    "print('Using:', orient, 'orientations', pix_per_cell, \n",
    "      'pixels per cell and', cell_per_block, 'cells per block')\n",
    "print('Feature vector length:', len(X_train[0]))\n",
    "\n",
    "# Use a linear SVC \n",
    "svc = LinearSVC()\n",
    "# Check the training time for the SVC\n",
    "t = time.time()\n",
    "svc.fit(X_train, y_train)\n",
    "t2 = time.time()\n",
    "print(round(t2 - t, 2), 'Seconds to train SVC...')\n",
    "# Check the score of the SVC\n",
    "print('Test Accuracy of SVC = ', round(svc.score(X_test, y_test), 4))\n",
    "# Check the prediction time for a single sample\n",
    "t=time.time()\n",
    "\n",
    "# Save data to pickle file\n",
    "dist_pickle = {}\n",
    "dist_pickle[\"svc\"] = svc\n",
    "dist_pickle[\"scaler\"] = X_scaler\n",
    "dist_pickle[\"orient\"] = orient\n",
    "dist_pickle[\"pix_per_cell\"] = pix_per_cell\n",
    "dist_pickle[\"cell_per_block\"] = cell_per_block\n",
    "dist_pickle[\"spatial_size\"] = spatial_size\n",
    "dist_pickle[\"hist_bins\"] = hist_bins\n",
    "pickle.dump(dist_pickle, open(\"svc_pickle.p\", 'wb') )\n",
    "\n",
    "print('Classifier parameters were saved to file')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "print('Loading Classifier parameters...')\n",
    "dist_pickle = pickle.load( open(\"svc_pickle.p\", \"rb\" ) )\n",
    "svc = dist_pickle[\"svc\"]\n",
    "X_scaler = dist_pickle[\"scaler\"]\n",
    "orient = dist_pickle[\"orient\"]\n",
    "pix_per_cell = dist_pickle[\"pix_per_cell\"]\n",
    "cell_per_block = dist_pickle[\"cell_per_block\"]\n",
    "spatial_size = dist_pickle[\"spatial_size\"]\n",
    "hist_bins = dist_pickle[\"hist_bins\"]\n",
    "\n",
    "print('Loading is done')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# Define a single function that can extract features using hog sub-sampling and make predictions\n",
    "def find_cars(img, svc, X_scaler, orient, pix_per_cell, cell_per_block, \n",
    "              spatial_size, hist_bins):\n",
    "    draw_img = np.copy(img)\n",
    "    img = img.astype(np.float32)/255\n",
    "    \n",
    "    ystart_ystop_scale = [(380, 480, 1), (400, 600, 1.5), (500, 700, 2.5)]\n",
    "    win_pos = []\n",
    "    # ADD MULTISCALING:\n",
    "    for (ystart, ystop, scale) in ystart_ystop_scale:\n",
    "        img_tosearch = img[ystart:ystop, :, :]\n",
    "        ctrans_tosearch = convert_color(img_tosearch, conv='RGB2YCrCb')\n",
    "        \n",
    "        if scale != 1:\n",
    "            imshape = ctrans_tosearch.shape\n",
    "            ctrans_tosearch = cv2.resize(ctrans_tosearch, (np.int(imshape[1]/scale), np.int(imshape[0]/scale)))\n",
    "        \n",
    "        ch1 = ctrans_tosearch[:,:,0]\n",
    "        ch2 = ctrans_tosearch[:,:,1]\n",
    "        ch3 = ctrans_tosearch[:,:,2]\n",
    "\n",
    "        # Define blocks and steps as above\n",
    "        nxblocks = ((ch1.shape[1] // pix_per_cell) - cell_per_block + 1)\n",
    "        nyblocks = ((ch1.shape[0] // pix_per_cell) - cell_per_block + 1)\n",
    "        nfeat_per_block = orient * cell_per_block**2\n",
    "    \n",
    "        # 64 was the orginal sampling rate, with 8 cells and 8 pix per cell\n",
    "        window = 8*8\n",
    "        nblocks_per_window = (window // pix_per_cell) - cell_per_block + 1\n",
    "        cells_per_step = 2  # Instead of overlap, define how many cells to step\n",
    "        nxsteps = (nxblocks - nblocks_per_window) // cells_per_step\n",
    "        nysteps = (nyblocks - nblocks_per_window) // cells_per_step\n",
    "    \n",
    "        # Compute individual channel HOG features for the entire image\n",
    "        hog1 = get_hog_features(ch1, orient, pix_per_cell, cell_per_block, feature_vec=False)\n",
    "        hog2 = get_hog_features(ch2, orient, pix_per_cell, cell_per_block, feature_vec=False)\n",
    "        hog3 = get_hog_features(ch3, orient, pix_per_cell, cell_per_block, feature_vec=False)\n",
    "    \n",
    "\n",
    "        for xb in range(nxsteps + 1):\n",
    "            for yb in range(nysteps + 1):\n",
    "                ypos = yb * cells_per_step\n",
    "                xpos = xb * cells_per_step\n",
    "                # Extract HOG for this patch\n",
    "                hog_feat1 = hog1[ypos:ypos+nblocks_per_window, xpos:xpos + nblocks_per_window].ravel() \n",
    "                hog_feat2 = hog2[ypos:ypos+nblocks_per_window, xpos:xpos + nblocks_per_window].ravel() \n",
    "                hog_feat3 = hog3[ypos:ypos+nblocks_per_window, xpos:xpos + nblocks_per_window].ravel() \n",
    "                hog_features = np.hstack((hog_feat1, hog_feat2, hog_feat3))\n",
    "\n",
    "                xleft = xpos * pix_per_cell\n",
    "                ytop = ypos * pix_per_cell\n",
    "\n",
    "                # Extract the image patch\n",
    "                subimg = cv2.resize(ctrans_tosearch[ytop:ytop + window, xleft:xleft + window], (64, 64))\n",
    "          \n",
    "                # Get color features\n",
    "                spatial_features = bin_spatial(subimg, size=spatial_size)\n",
    "                hist_features = color_hist(subimg, nbins=hist_bins)\n",
    "\n",
    "                # Scale features and make a prediction\n",
    "                test_features = X_scaler.transform(np.hstack((spatial_features, hist_features, hog_features)).reshape(1, -1))    \n",
    "                test_prediction = svc.predict(test_features)\n",
    "            \n",
    "                # If detected\n",
    "                if test_prediction == 1:\n",
    "                    xbox_left = np.int(xleft * scale)\n",
    "                    ytop_draw = np.int(ytop * scale)\n",
    "                    win_draw = np.int(window * scale)\n",
    "                    cv2.rectangle(draw_img, (xbox_left, ytop_draw + ystart), \n",
    "                                  (xbox_left + win_draw, ytop_draw + win_draw + ystart), (0,0,255), 2)\n",
    "                    win_pos.append(((xbox_left, ytop_draw + ystart), \n",
    "                                    (xbox_left + win_draw, ytop_draw + win_draw + ystart)))\n",
    "    return draw_img, win_pos\n",
    "\n",
    "def add_heat(heatmap, bbox_list):\n",
    "    # Iterate through list of bboxes\n",
    "    for box in bbox_list:\n",
    "        # Add += 1 for all pixels inside each bbox\n",
    "        # Assuming each \"box\" takes the form ((x1, y1), (x2, y2))\n",
    "        heatmap[box[0][1]:box[1][1], box[0][0]:box[1][0]] += 1\n",
    "    # Return updated heatmap\n",
    "    return heatmap\n",
    "\n",
    "def apply_threshold(heatmap, threshold):\n",
    "    # Zero out pixels below the threshold\n",
    "    heatmap[heatmap <= threshold] = 0\n",
    "    # Return thresholded map\n",
    "    return heatmap"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "# Accumulation of labels from last N frames\n",
    "class LabelsQueue():\n",
    "    def __init__ (self):\n",
    "        # Number labels to store\n",
    "        self.queue_len = 10 \n",
    "        self.queue = []\n",
    "\n",
    "    # Put new frame\n",
    "    def put_labels(self, labels):\n",
    "        if (len(self.queue) > self.queue_len):\n",
    "            tmp = self.queue.pop(0)\n",
    "        self.queue.append(labels)\n",
    "    \n",
    "    # Get last N frames hot boxes\n",
    "    def get_labels(self):\n",
    "        b = []\n",
    "        for label in self.queue:\n",
    "            b.extend(label)\n",
    "        return b"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "deletable": true,
    "editable": true
   },
   "outputs": [],
   "source": [
    "def detection_pipeline(img):\n",
    "    out_img, win_pos = find_cars(img, svc, X_scaler, orient, pix_per_cell, \n",
    "                                 cell_per_block, spatial_size, hist_bins)\n",
    "    # Read in image similar to one shown above \n",
    "    heat = np.zeros_like(img[:,:,0]).astype(np.float)\n",
    "    \n",
    "    # AVG boxes\n",
    "    last_hot_labels.put_labels(win_pos)\n",
    "    win_pos = last_hot_labels.get_labels()\n",
    "    \n",
    "    # Add heat to each box in box list\n",
    "    heat = add_heat(heat, win_pos)\n",
    "    # Apply threshold to help remove false positives\n",
    "    heat = apply_threshold(heat, 22)\n",
    "    # Visualize the heatmap when displaying\n",
    "    heatmap = np.clip(heat, 0, 255)\n",
    "    # Find final boxes from heatmap using label function\n",
    "    labels = label(heatmap)\n",
    "    draw_img = draw_labeled_bboxes(np.copy(img), labels)\n",
    "    \n",
    "    #plot_row2(img, out_img, 'Source', 'All Detections')\n",
    "    #plot_row2(draw_img, heatmap, 'Car Positions', 'Heat Map')\n",
    "    return labels, heatmap"
   ]
  },
  {
   "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",
    "    # Vehicle detection pipeline\n",
    "    labels, heatmap = detection_pipeline(undistorted)\n",
    "    # Draw output\n",
    "    proc_img = draw_res(warped, undistorted, out_img, ploty, left_fitx, right_fitx, \n",
    "                        left_curverad, right_curverad, lane_deviation, labels, heatmap)\n",
    "    return proc_img"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "deletable": true,
    "editable": true,
    "scrolled": 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",
    "    # Vehicle detection pipeline\n",
    "    labels, heatmap = detection_pipeline(undistorted)\n",
    "    # Draw output\n",
    "    proc_img = draw_res(warped, undistorted, out_img, ploty, left_fitx, right_fitx, \n",
    "                        left_curverad, right_curverad, lane_deviation, labels, heatmap)\n",
    "    #plot_row2(undistorted, proc_img, 'Undistorted', 'Result')\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",
    "last_hot_labels = LabelsQueue()\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",
    "last_hot_labels = LabelsQueue()\n",
    "output_video = 'project_video_processed-02.mp4'\n",
    "clip1 = VideoFileClip(\"project_video.mp4\")#.subclip(0,3)\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
}