Vehicle-Lane-Detection.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Vehicle Detection Pipeline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "C:\\Users\\Adam\\Anaconda3\\lib\\site-packages\\skimage\\feature\\_hog.py:119: skimage_deprecation: Default value of `block_norm`==`L1` is deprecated and will be changed to `L2-Hys` in v0.15\n",
      "  'be changed to `L2-Hys` in v0.15', skimage_deprecation)\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "length cars 8906\n",
      "length noncars 9047\n",
      "Using: 9 orientations 8 pixels per cell and 2 cells per block\n",
      "Feature vector length: 8460\n",
      "training data: 14362\n",
      "8.47 Seconds to train SVC...\n"
     ]
    }
   ],
   "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",
    "from sklearn.svm import LinearSVC\n",
    "from sklearn.preprocessing import StandardScaler\n",
    "from skimage.feature import hog\n",
    "from scipy.ndimage.measurements import label\n",
    "%matplotlib inline\n",
    "\n",
    "# NOTE: the next import is only valid for scikit-learn version <= 0.17\n",
    "# for scikit-learn >= 0.18 use:\n",
    "from sklearn.model_selection import train_test_split\n",
    "# from sklearn.cross_validation import train_test_split\n",
    "\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=True, \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",
    "#         print(np.any(np.isnan(img)))\n",
    "        return features\n",
    "\n",
    "# Define a function to compute binned color features  \n",
    "def bin_spatial(img, size=(32, 32)):\n",
    "    # Use cv2.resize().ravel() to create the feature vector\n",
    "    features = cv2.resize(img, size).ravel() \n",
    "    # Return the feature vector\n",
    "    return features\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, range=bins_range)\n",
    "    channel2_hist = np.histogram(img[:,:,1], bins=nbins, range=bins_range)\n",
    "    channel3_hist = np.histogram(img[:,:,2], bins=nbins, range=bins_range)\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",
    "# 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",
    "        feature_image = np.zeros_like(image)\n",
    "        feature_image = cv2.cvtColor(image, cv2.COLOR_RGB2YCrCb)\n",
    "#         feature_image = image\n",
    "#         r_rgb = image[:,:,0]\n",
    "#         s_hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)[:,:,1]\n",
    "#         u_luv = cv2.cvtColor(image, cv2.COLOR_RGB2LUV)[:,:,1]\n",
    "#         feature_image[:,:,0] = r_rgb\n",
    "#         feature_image[:,:,1] = s_hsv\n",
    "#         feature_image[:,:,2] = u_luv\n",
    "#         plt.figure()\n",
    "#         plt.imshow(feature_image)\n",
    "        # use original image in spacial features\n",
    "        if spatial_feat == True:\n",
    "            spatial_features = bin_spatial(feature_image, size=spatial_size)\n",
    "            \n",
    "#             file_features.append(spatial_features)\n",
    "        # use multiple color spaces for hist\n",
    "        if hist_feat == True:\n",
    "            # Apply color_hist()\n",
    "            hist_features = color_hist(feature_image, nbins=hist_bins)\n",
    "            \n",
    "#             file_features.append(hist_features)\n",
    "        # use multiple color spaces for hog\n",
    "        if hog_feat == True:\n",
    "        # Call get_hog_features() with vis=False, feature_vec=True\n",
    "#             print('feature Image', feature_image)\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",
    "                \n",
    "#             hog_features.append(get_hog_features(feature_image[:,:,0], \n",
    "#                     orient, pix_per_cell, cell_per_block, \n",
    "#                     vis=False, feature_vec=True))    \n",
    "                \n",
    "            hog_features = np.ravel(hog_features)        \n",
    "            \n",
    "            # Append the new feature vector to the features list\n",
    "#             file_features.append(hog_features)\n",
    "\n",
    "        features.append(np.concatenate((spatial_features, hist_features, hog_features)))\n",
    "    # Return list of feature vectors\n",
    "    return features\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "# cars1 = 'vehicles_smallset/cars1/'\n",
    "# cars2 = 'vehicles_smallset/cars1/'\n",
    "# cars3 = 'vehicles_smallset/cars3/'\n",
    "# notcar1 = 'non-vehicles_smallset/notcars1/'\n",
    "# notcar2 = 'non-vehicles_smallset/notcars2/'\n",
    "# notcar3 = 'non-vehicles_smallset/notcars3/'              \n",
    "        \n",
    "# carsF = [cars1, cars2, cars3]\n",
    "# cars = []\n",
    "# for car in carsF:\n",
    "#     cars.append(glob.glob(car + '*.jpeg'))   \n",
    "# cars= np.concatenate(cars)\n",
    "\n",
    "# notcarF = [notcar1, notcar2, notcar3]\n",
    "# notcars = []\n",
    "# for notcar in notcarF:\n",
    "#     notcars.append(glob.glob(notcar + '*.jpeg'))   \n",
    "# notcars = np.concatenate(notcars)  \n",
    "\n",
    "\n",
    "\n",
    "\n",
    "cars1 = 'vehicles/GTI_Far/'\n",
    "cars2 = 'vehicles/GTI_Left/'\n",
    "cars3 = 'vehicles/GTI_MiddleClose/'\n",
    "cars4 = 'vehicles/GTI_Right/'\n",
    "cars5 = 'vehicles/KITTI_extracted/'\n",
    "notcar1 = 'non-vehicles/GTI/'\n",
    "notcar2 = 'non-vehicles/Extras/'\n",
    "             \n",
    "        \n",
    "carsF = [cars1, cars2, cars3, cars4, cars5]\n",
    "cars = []\n",
    "for car in carsF:\n",
    "    cars.append(glob.glob(car + '*.png'))   \n",
    "cars= np.concatenate(cars)\n",
    "\n",
    "notcarF = [notcar1, notcar2]\n",
    "# notcarF = [notcar1]\n",
    "notcars = []\n",
    "for notcar in notcarF:\n",
    "    notcars.append(glob.glob(notcar + '*.png'))   \n",
    "notcars = np.concatenate(notcars) \n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "color_space = 'RGB' # 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 = \"0\" # 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",
    "\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",
    "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",
    "\n",
    "print('length cars', len(car_features))\n",
    "print('length noncars', len(notcar_features))\n",
    "\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",
    "\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",
    "# X_train = scaled_X\n",
    "# y_train = y\n",
    "\n",
    "\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",
    "print('training data:', len(X_train))\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()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Define a function that takes an image,\n",
    "# start and stop positions in both x and y, \n",
    "# window size (x and y dimensions),  \n",
    "# and overlap fraction (for both x and y)\n",
    "def slide_window(img, x_start_stop=[None, None], y_start_stop=[None, None], \n",
    "                    xy_window=(64, 64), xy_overlap=(0.5, 0.5)):\n",
    "    # If x and/or y start/stop positions not defined, set to image size\n",
    "    if x_start_stop[0] == None:\n",
    "        x_start_stop[0] = 0\n",
    "    if x_start_stop[1] == None:\n",
    "        x_start_stop[1] = img.shape[1]\n",
    "    if y_start_stop[0] == None:\n",
    "        y_start_stop[0] = 0\n",
    "    if y_start_stop[1] == None:\n",
    "        y_start_stop[1] = img.shape[0]\n",
    "    # Compute the span of the region to be searched    \n",
    "    xspan = x_start_stop[1] - x_start_stop[0]\n",
    "    yspan = y_start_stop[1] - y_start_stop[0]\n",
    "    # Compute the number of pixels per step in x/y\n",
    "    nx_pix_per_step = np.int(xy_window[0]*(1 - xy_overlap[0]))\n",
    "    ny_pix_per_step = np.int(xy_window[1]*(1 - xy_overlap[1]))\n",
    "    # Compute the number of windows in x/y\n",
    "    nx_buffer = np.int(xy_window[0]*(xy_overlap[0]))\n",
    "    ny_buffer = np.int(xy_window[1]*(xy_overlap[1]))\n",
    "    nx_windows = np.int((xspan-nx_buffer)/nx_pix_per_step) \n",
    "    ny_windows = np.int((yspan-ny_buffer)/ny_pix_per_step) \n",
    "    # Initialize a list to append window positions to\n",
    "    window_list = []\n",
    "    # Loop through finding x and y window positions\n",
    "    # Note: you could vectorize this step, but in practice\n",
    "    # you'll be considering windows one by one with your\n",
    "    # classifier, so looping makes sense\n",
    "    for ys in range(ny_windows):\n",
    "        for xs in range(nx_windows):\n",
    "            # Calculate window position\n",
    "            startx = xs*nx_pix_per_step + x_start_stop[0]\n",
    "            endx = startx + xy_window[0]\n",
    "            starty = ys*ny_pix_per_step + y_start_stop[0]\n",
    "            endy = starty + xy_window[1]\n",
    "            \n",
    "            # Append window position to list\n",
    "            window_list.append(((startx, starty), (endx, endy)))\n",
    "    # Return the list of windows\n",
    "    return window_list\n",
    "\n",
    "# Define a function to draw bounding boxes\n",
    "def draw_boxes(img, bboxes, color=(0, 0, 255), thick=6):\n",
    "    # Make a copy of the image\n",
    "    imcopy = np.copy(img)\n",
    "    # Iterate through the bounding boxes\n",
    "    for bbox in bboxes:\n",
    "        # Draw a rectangle given bbox coordinates\n",
    "        cv2.rectangle(imcopy, bbox[0], bbox[1], color, thick)\n",
    "    # Return the image copy with boxes drawn\n",
    "    return imcopy\n",
    "\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",
    "\n",
    "#     features = []\n",
    "#     feature_image = np.zeros_like(img)\n",
    "#     r_rgb = img[:,:,0]\n",
    "#     s_hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)[:,:,1]\n",
    "#     u_luv = cv2.cvtColor(img, cv2.COLOR_RGB2LUV)[:,:,1]\n",
    "#     feature_image[:,:,0] = r_rgb\n",
    "#     feature_image[:,:,1] = s_hsv\n",
    "#     feature_image[:,:,2] = u_luv\n",
    "        \n",
    "    features = []\n",
    "    feature_image = np.zeros_like(img)\n",
    "    feature_image = cv2.cvtColor(img, cv2.COLOR_RGB2YCrCb)\n",
    "    \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",
    "        #8) Append features to list\n",
    "#         img_features.append(hog_features)\n",
    "\n",
    "    features.append(np.concatenate((spatial_features, hist_features, hog_features)))\n",
    "    #9) Return concatenated array of features\n",
    "    return features\n",
    "\n",
    "\n",
    "# Define a function you will pass an image \n",
    "# and the list of windows to be searched (output of slide_windows())\n",
    "def search_windows(img, windows, clf, scaler, color_space='RGB', \n",
    "                    spatial_size=(32, 32), hist_bins=32, \n",
    "                    hist_range=(0, 256), orient=9, \n",
    "                    pix_per_cell=8, cell_per_block=2, \n",
    "                    hog_channel=0, spatial_feat=True, \n",
    "                    hist_feat=True, hog_feat=True):\n",
    "    \n",
    "    #1) Create an empty list to receive positive detection windows\n",
    "    on_windows = []\n",
    "    #2) Iterate over all windows in the list\n",
    "    for window in windows:\n",
    "        if window:\n",
    "            #3) Extract the test window from original image\n",
    "            test_img = cv2.resize(img[window[0][1]:window[1][1], window[0][0]:window[1][0]], (64, 64))      \n",
    "            #4) Extract features for that window using single_img_features()\n",
    "            features = single_img_features(test_img, 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",
    "            #5) Scale extracted features to be fed to classifier\n",
    "            test_features = scaler.transform(np.array(features).reshape(1, -1))\n",
    "            #6) Predict using your classifier\n",
    "            prediction = clf.predict(test_features)\n",
    "            #7) If positive (prediction == 1) then save the window\n",
    "            if prediction == 1:\n",
    "                on_windows.append(window)\n",
    "    #8) Return windows for positive detections\n",
    "    return on_windows\n",
    "\n",
    "\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",
    "\n",
    "    # Return updated heatmap\n",
    "    return heatmap# Iterate through list of bboxes\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\n",
    "\n",
    "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",
    "\n",
    "\n",
    "# # Read in Video Frames as Images\n",
    "# images = glob.glob('Data/'+'*.jpg')\n",
    "\n",
    "# for image in images[300:400]:\n",
    "#     image = mpimg.imread(image)\n",
    "#     result = find_vehicles(image)\n",
    "# #     plt.figure()\n",
    "# #     plt.imshow(result)\n",
    "\n",
    "# # labels then heatmap"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Lane Detection Defs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# This method takes an image and the desired gradent orientation\n",
    "# and comutes a sobel edge detected image in that gradient direction.\n",
    "def abs_sobel_thresh(img, orient='x', sobel_kernel=3, thresh=(0, 255)):\n",
    "\n",
    "    # Convert to grayscale\n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)# cv2\n",
    "    \n",
    "    # Apply x or y gradient with the OpenCV Sobel() function\n",
    "    # and take the absolute value\n",
    "\n",
    "    if orient == 'x':\n",
    "        abs_sobel = np.absolute(cv2.Sobel(gray, cv2.CV_64F, 1, 0,ksize=sobel_kernel))\n",
    "\n",
    "    if orient == 'y':\n",
    "        abs_sobel = np.absolute(cv2.Sobel(gray, cv2.CV_64F, 0, 1,ksize=sobel_kernel))\n",
    "        \n",
    "    \n",
    "    # Rescale back to 8 bit integer\n",
    "    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))\n",
    "\n",
    "    # Create a copy and apply the threshold\n",
    "    binary_output = np.zeros_like(scaled_sobel)\n",
    "    # Here I'm using inclusive (>=, <=) thresholds, but exclusive is ok too\n",
    "    binary_output[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1  \n",
    "\n",
    "    return binary_output\n",
    "\n",
    "# This method calculates the sobel in both x and y \n",
    "# and filteres out gradients below a certain magnitude\n",
    "def mag_thresh(image, sobel_kernel=3, mag_thresh=(0, 255)):\n",
    "    # Calculate gradient magnitude\n",
    "    # Apply threshold\n",
    "    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)#cv2\n",
    "    \n",
    "    # Take both Sobel x and y gradients\n",
    "    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)\n",
    "    sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)\n",
    "    # Calculate the gradient magnitude\n",
    "    gradmag = np.sqrt(sobelx**2 + sobely**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 image of ones where threshold is met, zeros otherwise\n",
    "    binary_output = np.zeros_like(gradmag)\n",
    "    binary_output[(gradmag >= mag_thresh[0]) & (gradmag <= mag_thresh[1])] = 1\n",
    "    return binary_output\n",
    "\n",
    "# gradients outside of an expected range of line gradients are thresholded out\n",
    "def dir_threshold(image, sobel_kernel=3, thresh=(0, np.pi/2)):\n",
    "    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)\n",
    "    # Calculate the x and y gradients\n",
    "    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)\n",
    "    sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)\n",
    "    # Take the absolute value of the gradient direction, \n",
    "    # apply a threshold, and create a binary image result\n",
    "    absgraddir = np.arctan2(np.absolute(sobely), np.absolute(sobelx))\n",
    "    binary_output =  np.zeros_like(absgraddir)\n",
    "    binary_output[(absgraddir >= thresh[0]) & (absgraddir <= thresh[1])] = 1\n",
    "    return binary_output\n",
    "\n",
    "# a hugh light saturation image s chanel is used to pick up lane markings\n",
    "def hls_threshold(image, s_thresh=(170, 255)):   \n",
    "    hls = cv2.cvtColor(image, cv2.COLOR_BGR2HLS).astype(np.float)\n",
    "    s_channel = hls[:,:,2]\n",
    "    # Threshold saturation channel\n",
    "    s_binary = np.zeros_like(s_channel)\n",
    "    s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1    \n",
    "    return s_binary\n",
    "\n",
    "# the red channel on the RGB spectrum was seen to be effetive in distinquishing the lane markings\n",
    "def red_threshold(image, r_thresh=(170, 255)):   \n",
    "\n",
    "    r_channel = image[:,:,2] # using cv2 to read\n",
    "    # Threshold saturation channel\n",
    "    r_binary = np.zeros_like(r_channel)\n",
    "    r_binary[(r_channel >= r_thresh[0]) & (r_channel <= r_thresh[1])] = 1    \n",
    "    return r_binary\n",
    "\n",
    "   \n",
    "\n",
    "\n",
    "def pipeline(image):\n",
    "\n",
    "    \n",
    "    # Choose a Sobel kernel size\n",
    "    ksize = 15 # Choose a larger odd number to smooth gradient measurements\n",
    "\n",
    "    # Apply each of the thresholding functions\n",
    "    gradx = abs_sobel_thresh(image, orient='x', sobel_kernel=ksize, thresh=(100, 255))\n",
    "    grady = abs_sobel_thresh(image, orient='y', sobel_kernel=ksize, thresh=(130, 255))\n",
    "    mag_binary = mag_thresh(image, sobel_kernel=ksize, mag_thresh=(70, 255))\n",
    "    dir_binary = dir_threshold(image, sobel_kernel=ksize, thresh=(np.pi/2-1, np.pi/2-.35))\n",
    "    s_binary = hls_threshold(image, s_thresh=(150, 255))\n",
    "    red_binary = red_threshold(image, r_thresh=(200, 255))\n",
    "    combined = np.zeros_like(gradx)\n",
    "    combined[((gradx == 1) | (grady == 1)) | ((mag_binary == 1) & (dir_binary == 1))|((red_binary == 1)&(s_binary == 1))] = 1\n",
    "\n",
    "    return combined\n",
    "\n",
    "\n",
    "\n",
    "# the below function takes in a set of vertices and masks out all pixels outside of this polygon\n",
    "# The mask region was made slightly larger than the lane itself to insure sharper curves were also included\n",
    "def maskRegion(image, vertices):\n",
    "    #defining a blank mask to start with\n",
    "    mask = np.zeros_like(image) \n",
    "    \n",
    "    #defining a 3 channel or 1 channel color to fill the mask with depending on the input image\n",
    "    if len(image.shape) > 2:\n",
    "        channel_count = image.shape[2]  # i.e. 3 or 4 depending on your image\n",
    "        ignore_mask_color = (255,) * channel_count\n",
    "    else:\n",
    "        ignore_mask_color = 255 \n",
    "\n",
    "    #filling pixels inside the polygon defined by \"vertices\" with the fill color    \n",
    "    cv2.fillPoly(mask, vertices, ignore_mask_color)\n",
    "    \n",
    "    #returning the image only where mask pixels are nonzero\n",
    "    masked_image = cv2.bitwise_and(image, mask)\n",
    "    \n",
    "    return masked_image\n",
    "\n",
    "\n",
    "# This method returns the perspective transform matrix and its inverse\n",
    "def get_M_Minv(img):\n",
    "    img_size = (img.shape[1],img.shape[0])  \n",
    "    src = np.float32(\n",
    "    [[578, 460],\n",
    "    [200, 720],\n",
    "    [1090, 720],\n",
    "    [692, 460]])  \n",
    "    dst = np.float32(\n",
    "    [[320, 0],\n",
    "    [320, 720],\n",
    "    [960, 720],\n",
    "    [960, 0]])\n",
    "    # compute perspective transform\n",
    "    M = cv2.getPerspectiveTransform(src,dst)    \n",
    "    Minv = cv2.getPerspectiveTransform(dst,src)   \n",
    "    return M, Minv\n",
    "\n",
    "# gets perspective transformed image\n",
    "def warp(img, M):\n",
    "    img_size = (img.shape[1],img.shape[0])   \n",
    "    warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)\n",
    "    return warped\n",
    "\n",
    "# unwarps the image back to original\n",
    "def unWarp(img, Minv):\n",
    "    img_size = (img.shape[1],img.shape[0])    \n",
    "    warped = cv2.warpPerspective(img, Minv, img_size, flags=cv2.INTER_LINEAR)\n",
    "    return unWarped\n",
    "\n",
    "\n",
    "# This methods determines where the base of the lane is using a histogram\n",
    "def getHist(warped):\n",
    "    histogram = np.sum(warped[int(warped.shape[0]/2):,:], axis=0)\n",
    "    return histogram\n",
    "\n",
    "def extractForFit(warped,LtLine, RtLine): \n",
    "    \n",
    "    histogram = getHist(warped)\n",
    "    midpoint = np.int(histogram.shape[0]/2)\n",
    "    nwindows = 9\n",
    "    # Set height of windows\n",
    "    window_height = np.int(warped.shape[0]/nwindows)\n",
    "    nonzero = warped.nonzero()\n",
    "    nonzeroy = np.array(nonzero[0])\n",
    "    nonzerox = np.array(nonzero[1])    \n",
    "    # Set the width of the windows +/- margin\n",
    "    margin = 100\n",
    "    # Set minimum number of pixels found to recenter window\n",
    "    minpix = 30\n",
    "    # Create empty lists to receive left and right lane pixel indices\n",
    "    left_lane_inds = []\n",
    "    right_lane_inds = []\n",
    "\n",
    "  \n",
    "    if not LtLine.detected:\n",
    "        leftx_base = np.argmax(histogram[:midpoint])\n",
    "        leftx_current = leftx_base\n",
    "        # Step through the windows one by one\n",
    "        for window in range(nwindows):        \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",
    "            # 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",
    "\n",
    "            # Append these indices to the lists\n",
    "            left_lane_inds.append(good_left_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",
    "        # Concatenate the arrays of indices\n",
    "        left_lane_inds = np.concatenate(left_lane_inds)\n",
    "    \n",
    "    else:\n",
    "#         left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy+ left_fit[2] - margin)) & \\\n",
    "#                           (nonzerox < (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + left_fit[2] + margin)))\n",
    "        left_lane_inds = ((nonzerox > (LtLine.best_fit[0]*(nonzeroy**2) + \\\n",
    "                                       LtLine.best_fit[1]*nonzeroy+ LtLine.best_fit[2] - margin)) & \\\n",
    "                  (nonzerox < (LtLine.best_fit[0]*(nonzeroy**2) + \\\n",
    "                               LtLine.best_fit[1]*nonzeroy + LtLine.best_fit[2] + margin)))\n",
    "\n",
    "        \n",
    "        \n",
    "    if not RtLine.detected:\n",
    "        rightx_base = np.argmax(histogram[midpoint:])+ midpoint\n",
    "        rightx_current = rightx_base\n",
    "        # Step through the windows one by one\n",
    "        for window in range(nwindows):        \n",
    "            win_y_low = warped.shape[0] - (window+1)*window_height\n",
    "            win_y_high = warped.shape[0] - window*window_height\n",
    "            win_xright_low = rightx_current - margin\n",
    "            win_xright_high = rightx_current + margin\n",
    "            \n",
    "            # Identify the nonzero pixels in x and y within the window\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",
    "            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_right_inds) > minpix:\n",
    "                rightx_current = np.int(np.mean(nonzerox[good_right_inds]))\n",
    "        right_lane_inds = np.concatenate(right_lane_inds)\n",
    "    else:\n",
    "        right_lane_inds = ((nonzerox > (RtLine.best_fit[0]*(nonzeroy**2) + RtLine.best_fit[1]*nonzeroy + RtLine.best_fit[2] - margin)) \\\n",
    "                           & (nonzerox < (RtLine.best_fit[0]*(nonzeroy**2) + RtLine.best_fit[1]*nonzeroy + RtLine.best_fit[2] + margin)))  \n",
    "\n",
    "\n",
    "\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",
    "     \n",
    "\n",
    "    \n",
    "    return leftx, lefty, rightx, righty           \n",
    "       \n",
    "\n",
    "\n",
    "\n",
    "\n",
    "# Class containing 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 = [] \n",
    "        # y values of the last n fits of the line\n",
    "        self.recent_yfitted = [] \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",
    "        #previos frame best fit\n",
    "        self.prevbest_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\n",
    "        self.radius_of_curvature = None         \n",
    "        #Location where line hits bottom of image\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 in last n frames\n",
    "        self.arrayx = None  \n",
    "        #y values for detected line pixels in last n frames\n",
    "        self.array = None\n",
    "        \n",
    "        \n",
    "# This function take drawes a shaded green region to mark the lane\n",
    "def markLane(image, warped, left_fit,right_fit):\n",
    "    # y points to mark line\n",
    "    ploty = np.linspace(0, image.shape[0]-1, num=image.shape[0])\n",
    "    left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]\n",
    "    right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]\n",
    "    color_warp = np.zeros_like(image).astype(np.uint8)\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",
    "\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",
    "\n",
    "    # Warp the blank back to original image space using inverse perspective matrix (Minv)\n",
    "    newwarp = cv2.warpPerspective(color_warp, Minv, (image.shape[1], image.shape[0])) \n",
    "    # Combine the result with the original image\n",
    "    result = cv2.addWeighted(image, 1, newwarp, 0.3, 0)\n",
    "\n",
    "    return result\n",
    "\n",
    "\n",
    "\n",
    "def lane_pipelineV(image):\n",
    "    global loopn\n",
    "    global curLtLine\n",
    "    global curRtLine\n",
    "    global offCenter\n",
    "    global framesL\n",
    "    \n",
    "        # undistor image\n",
    "    image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)\n",
    "    image = cv2.undistort(image, mtx, dist, None, mtx)\n",
    "    # detect prospective lines\n",
    "    combined = pipeline(image)\n",
    "    # mask out regions outside of expected lane region\n",
    "    masked = maskRegion(combined,vertices)  \n",
    "    # perfor merspective transform\n",
    "    warped = warp(masked, M)\n",
    "    \n",
    "    # extract left and right pixel location of perspective lane lines\n",
    "    leftx, lefty, rightx, righty = extractForFit(warped, curLtLine, curRtLine)\n",
    "    # get a 2nd order polynomial fit for prospective left and right lines\n",
    "    \n",
    "    \n",
    "    if len(leftx)>500:\n",
    "\n",
    "        curLtLine.current_fit = np.polyfit(lefty,leftx, 2)\n",
    "        curLtLine.detected = True\n",
    "    else:\n",
    "        curLtLine.detected = False  \n",
    "        \n",
    "    if len(rightx)>500:\n",
    "        curRtLine.current_fit = np.polyfit(righty,rightx, 2)\n",
    "        curRtLine.detected = True\n",
    "    else:\n",
    "        curRtLine.detected = False \n",
    "\n",
    "    \n",
    "    if curLtLine.detected:    \n",
    "        # keep last n frames of data for poly fit\n",
    "        if(len(curLtLine.recent_xfitted)<framesL):\n",
    "            curLtLine.recent_xfitted.append(leftx)\n",
    "            curLtLine.recent_yfitted.append(lefty)\n",
    "        else:\n",
    "            curLtLine.recent_xfitted[loopn%framesL] =leftx \n",
    "            curLtLine.recent_yfitted[loopn%framesL] =lefty\n",
    "\n",
    "        curLtLine.arrayx = np.concatenate(curLtLine.recent_xfitted)\n",
    "        curLtLine.arrayy = np.concatenate(curLtLine.recent_yfitted)\n",
    "#         curLtLine.best_fit = np.polyfit(curLtLine.arrayy,\\\n",
    "#                                     curLtLine.arrayx, 2)\n",
    "        tempFitL = np.polyfit(curLtLine.arrayy,\\\n",
    "                                    curLtLine.arrayx, 2)\n",
    "\n",
    "        left_base = tempFitL[0]*y_eval**2 + tempFitL[1]*y_eval + tempFitL[2]\n",
    "        # curvature in meters\n",
    "        left_fit_m_temp = np.polyfit(curLtLine.arrayy*ym_per_pix, \\\n",
    "                                curLtLine.arrayx*xm_per_pix, 2)\n",
    "        temp_L_curve= ((1 + (2*left_fit_m_temp[0]*y_eval*ym_per_pix+left_fit_m_temp[1])**2)\\\n",
    "                       **1.5) /np.absolute(2*left_fit_m_temp[0])\n",
    "\n",
    "        \n",
    "    if curRtLine.detected:\n",
    "        # keep last n frames of data for poly fit\n",
    "        if(len(curRtLine.recent_xfitted)<framesL):\n",
    "            curRtLine.recent_xfitted.append(rightx)\n",
    "            curRtLine.recent_yfitted.append(righty)\n",
    "        else:\n",
    "            curRtLine.recent_xfitted[loopn%framesL] =rightx \n",
    "            curRtLine.recent_yfitted[loopn%framesL] =righty\n",
    "            \n",
    "        curRtLine.arrayx = np.concatenate(curRtLine.recent_xfitted)\n",
    "        curRtLine.arrayy = np.concatenate(curRtLine.recent_yfitted)\n",
    "        tempFitR = np.polyfit(curRtLine.arrayy,\\\n",
    "                                        curRtLine.arrayx, 2)\n",
    "        right_base = tempFitR[0]*y_eval**2 + tempFitR[1]*y_eval + tempFitR[2] \n",
    "        \n",
    "        right_fit_m_temp = np.polyfit(curRtLine.arrayy*ym_per_pix, \\\n",
    "                                curRtLine.arrayx*xm_per_pix, 2)\n",
    "        \n",
    "        temp_R_curve= ((1 + (2*right_fit_m_temp[0]*y_eval*ym_per_pix+right_fit_m_temp[1])**2)\\\n",
    "                       **1.5) /np.absolute(2*right_fit_m_temp[0])\n",
    "\n",
    "\n",
    "    # Initial loop\n",
    "    if loopn is 0 or curRtLine.best_fit is None or curLtLine.best_fit is None:\n",
    "        if curLtLine.detected:\n",
    "            curLtLine.best_fit = tempFitL\n",
    "            curLtLine.line_base_pos = left_base\n",
    "            curLtLine.radius_of_curvature = temp_L_curve\n",
    "            left_fit_m = left_fit_m_temp\n",
    "        if curRtLine.detected:\n",
    "            curRtLine.best_fit = tempFitR\n",
    "            right_fit_m = right_fit_m_temp\n",
    "            curRtLine.radius_of_curvature = temp_R_curve\n",
    "            curRtLine.line_base_pos = right_base      \n",
    "    elif curRtLine.detected and curLtLine.detected and curLtLine.best_fit is not None and curRtLine.best_fit is not None:\n",
    "        laneWidthPx = abs(right_base - left_base)\n",
    "        # if lane to small or too big only update line that is closest to previous line\n",
    "        # or if the difference in radius changes by more than 10% of previous only update the closest line\n",
    "        if laneWidthPx*xm_per_pix < 3 or laneWidthPx*xm_per_pix > 4.4 or\\\n",
    "            abs(temp_R_curve - temp_L_curve) >1.03*abs(curLtLine.radius_of_curvature-curRtLine.radius_of_curvature):\n",
    "            leftChange = LA.norm(curLtLine.best_fit - curLtLine.current_fit)\n",
    "            rightChange = LA.norm(curRtLine.best_fit - curRtLine.current_fit)\n",
    "            if leftChange > rightChange :\n",
    "                curLtLine.detected = False # false set here so next frame recognizes previos line not good\n",
    "                curRtLine.best_fit = tempFitR\n",
    "                right_fit_m = right_fit_m_temp\n",
    "                curRtLine.radius_of_curvature = temp_R_curve\n",
    "                curRtLine.line_base_pos = right_base\n",
    "            else:\n",
    "                curRtLine.detected = False\n",
    "                curLtLine.best_fit = tempFitL\n",
    "                left_fit_m = left_fit_m_temp\n",
    "                curLtLine.radius_of_curvature = temp_L_curve\n",
    "                curLtLine.line_base_pos = left_base\n",
    "        else:\n",
    "            curRtLine.best_fit = tempFitR\n",
    "            right_fit_m = right_fit_m_temp        \n",
    "            curLtLine.best_fit = tempFitL\n",
    "            left_fit_m = left_fit_m_temp        \n",
    "            curRtLine.radius_of_curvature = temp_R_curve\n",
    "            curLtLine.radius_of_curvature = temp_L_curve\n",
    "            curRtLine.line_base_pos = right_base\n",
    "            curLtLine.line_base_pos = left_base\n",
    "    \n",
    "#     loopn = loopn+1\n",
    "    if curLtLine.best_fit is not None and curRtLine.best_fit is not None:\n",
    "        laneWidthPx = curRtLine.line_base_pos - curLtLine.line_base_pos\n",
    "        lane_center = laneWidthPx/2+curLtLine.line_base_pos\n",
    "        offCenter = (lane_center - imgmid)*xm_per_pix        \n",
    "        sign = np.sign(offCenter)\n",
    "\n",
    "        if sign>0:\n",
    "            side = \"Vehicle Right of Center by \"+str(offCenter)+\"m\"\n",
    "        else:\n",
    "            side = \"Vehicle Left of Center by \"+str(offCenter)+\"m\"\n",
    "        \n",
    "        if(curLtLine.best_fit[0] - curLtLine.line_base_pos)>0:\n",
    "            direct = \"Right\"\n",
    "        else:\n",
    "            direct = \"Left\"\n",
    "        radiusWords = \"Radius of curvature is \"+str(curLtLine.radius_of_curvature) + \"m to the \" + str(direct)\n",
    "        result = markLane(image, warped, curLtLine.best_fit, curRtLine.best_fit)#should use best fit\n",
    "#         cv2.putText(result, side, (50,100), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),4)\n",
    "#         cv2.putText(result, radiusWords, (50,200), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),4)  \n",
    "        \n",
    "        result = cv2.cvtColor(result, cv2.COLOR_BGR2RGB)\n",
    "        return result\n",
    "        image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)\n",
    "    return image\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[MoviePy] >>>> Building video Vehicles_Detected.mp4\n",
      "[MoviePy] Writing video Vehicles_Detected.mp4\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\r",
      "  0%|                                                                                         | 0/1261 [00:00<?, ?it/s]"
     ]
    }
   ],
   "source": [
    "import os\n",
    "\n",
    "import numpy as np\n",
    "from numpy import linalg as LA\n",
    "import cv2\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.image as mpimg\n",
    "\n",
    "# Camera calibration matrices\n",
    "mtx= np.array([[  1.15396093e03,   0.00000000e00,   6.69705359e02],\\\n",
    "                [  0.00000000e00,   1.14802495e03,   3.85656232e02],\\\n",
    "                [  0.00000000e00,   0.00000000e00,   1.00000000e00]])\n",
    "dist = np.array([[ -2.41017968e-01 , -5.30720497e-02,\\\n",
    "                  -1.15810318e-03,  -1.28318543e-04,   2.67124302e-02]])\n",
    "\n",
    "offCenter = 0 # in meters the distance the car is from the center of the lane\n",
    "curLtLine = Line()\n",
    "curRtLine = Line()\n",
    "framesL=4\n",
    " \n",
    "# loopn = 0 \n",
    "test = 'test_images/bbox-example-image.jpg'\n",
    "image = cv2.imread(test)\n",
    "# get perspective transform matrices\n",
    "M, Minv = get_M_Minv(image)\n",
    "img_size = (image.shape[1],image.shape[0])\n",
    "# bottom left, to top left, to top right, to bottom right\n",
    "vertices = np.array([[((img_size[0] / 6 - 130), img_size[1]),\\\n",
    "                     ((img_size[0] / 2) - 85, img_size[1] / 2 + 100),\\\n",
    "                     ((img_size[0] / 2 + 85), img_size[1] / 2 + 100),\\\n",
    "                     ((img_size[0] * 5 / 6) + 130, img_size[1])]],dtype=np.int32)\n",
    "# of pixels per meter to get radius in meters\n",
    "ym_per_pix = 30/720\n",
    "xm_per_pix = 3.7/700\n",
    "\n",
    "# evaluate curvature \n",
    "y_eval =image.shape[0]\n",
    "imgmid = image.shape[1]/2\n",
    "\n",
    "\n",
    "\n",
    "box_sizes = [(256,256),(200,200),(164,164),(128,128),(96,96)]\n",
    "y_start_stop = [[380, 700],[360,600],[380, 550],[380, 550],[400,500]]\n",
    "overlaps = [(.5,.5),(.5,.7),(.7,.7),(.9,.4),(.8,.8)]\n",
    "# changed search region from 350 to 400 and reduced threshold to 1\n",
    "\n",
    "\n",
    "image = mpimg.imread('test_images/frame0.jpg')\n",
    "windows = []\n",
    "for b_size, y_lim,overlap in zip(box_sizes,y_start_stop, overlaps):\n",
    "    windows.extend(slide_window(image, x_start_stop=[None, None], y_start_stop=y_lim, \n",
    "                        xy_window=b_size, xy_overlap=overlap))\n",
    "\n",
    "frames = 2\n",
    "box_group  = []\n",
    "loopn = 0\n",
    "def find_vehicles_lanes(img):\n",
    "    global box_group\n",
    "    global loopn\n",
    "    global windows  \n",
    "    # find lane_markings\n",
    "    lane_img =lane_pipelineV(img)\n",
    "    \n",
    "\n",
    "    box_list = []\n",
    "    hot_windows = []\n",
    "#     image = img\n",
    "    image = img.astype(np.float32)/255\n",
    "    hot_windows = search_windows(image, windows, svc, X_scaler, 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",
    "    heat = np.zeros_like(image[:,:,0]).astype(np.float)\n",
    "#     box_list = hot_windows\n",
    "\n",
    "    if hot_windows:\n",
    "        if(len(box_group)<frames):\n",
    "            box_group.append(hot_windows)\n",
    "        else:\n",
    "            box_group[loopn%frames] = hot_windows \n",
    "    for group in box_group:\n",
    "        box_list.extend(group)\n",
    "\n",
    "    # Add heat to each box in box list\n",
    "    heat = add_heat(heat,box_list)\n",
    "    heatmap2 = np.clip(heat, 0, 255)\n",
    "    # Apply threshold to help remove false positives\n",
    "    heat = apply_threshold(heat,3)\n",
    "\n",
    "    # Visualize the heatmap when displaying    \n",
    "    heatmap = np.clip(heat, 0, 255)\n",
    "\n",
    "    # Find final boxes from heatmap using label function\n",
    "    labels = label(heatmap)\n",
    "    draw_img = draw_labeled_bboxes(np.copy(lane_img), labels)\n",
    "    loopn = loopn +1\n",
    "    \n",
    "#     fig = plt.figure()\n",
    "#     plt.subplot(121)\n",
    "#     plt.imshow(draw_img)\n",
    "#     plt.title('Car Positions')\n",
    "#     plt.subplot(122)\n",
    "#     plt.imshow(heatmap2, cmap='hot')\n",
    "#     plt.title('Heat Map')\n",
    "#     fig.tight_layout()\n",
    "    \n",
    "   \n",
    "    return draw_img\n",
    "\n",
    "from moviepy.editor import VideoFileClip\n",
    "from IPython.display import HTML\n",
    "\n",
    "outputVid8 = 'Vehicles_Detected.mp4'\n",
    "clip = VideoFileClip(\"original_highway.mp4\")\n",
    "lane_clip = clip.fl_image(find_vehicles_lanes) #NOTE: this function expects color images!!\n",
    "lane_clip.write_videofile(outputVid8, audio=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "<video width=\"960\" height=\"540\" controls>\n",
       "  <source src=\"Vehicles_Detected8.mp4\">\n",
       "</video>\n"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "HTML(\"\"\"\n",
    "<video width=\"960\" height=\"540\" controls>\n",
    "  <source src=\"{0}\">\n",
    "</video>\n",
    "\"\"\".format(outputVid8))"
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [default]",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}