main.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Advanced Lane Finding Project\n",
    "\n",
    "The goals / steps of this project are the following:\n",
    "\n",
    "* Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.\n",
    "* Apply a distortion correction to raw images.\n",
    "* Use color transforms, gradients, etc., to create a thresholded binary image.\n",
    "* Apply a perspective transform to rectify binary image (\"birds-eye view\").\n",
    "* Detect lane pixels and fit to find the lane boundary.\n",
    "* Determine the curvature of the lane and vehicle position with respect to center.\n",
    "* Warp the detected lane boundaries back onto the original image.\n",
    "* Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.\n",
    "\n",
    "---\n",
    "## First, I'll compute the camera calibration using chessboard images"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import glob\n",
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.image as mpimg\n",
    "import os\n",
    "from itertools import groupby, islice,  cycle\n",
    "\n",
    "%matplotlib qt\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def visualize(filename, a):\n",
    "    fig, axes = plt.subplots(2,4,figsize=(24,12),subplot_kw={'xticks':[],'yticks':[]})\n",
    "    fig.subplots_adjust(hspace=0.03, wspace=0.05)\n",
    "    for p in zip(sum(axes.tolist(),[]), a):\n",
    "        p[0].imshow(p[1],cmap='gray')\n",
    "    plt.tight_layout()\n",
    "    fig.savefig(filename)\n",
    "    plt.close()\n",
    "    \n",
    "visualize(\"output_images/grid_images/images_grid_test.jpg\",\n",
    "          (mpimg.imread(f) for f in cycle(glob.glob(\"test_images/test*.jpg\"))))\n",
    "\n",
    "def plot_all(src, dst, src_title, dst_title):\n",
    "        f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))\n",
    "        f.tight_layout()\n",
    "        ax1.imshow(src, cmap='gray')\n",
    "        ax1.set_title(src_title, fontsize=50)\n",
    "        ax2.imshow(dst, cmap='gray')\n",
    "        ax2.set_title(dst_title, fontsize=50)\n",
    "        plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_camera_calibration(img_size):\n",
    "    objp = np.zeros((6*9,3), np.float32)\n",
    "    objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)\n",
    "\n",
    "    objpoints = [] # 3d points in real world space\n",
    "    imgpoints = [] # 2d points in image plane.\n",
    "\n",
    "    images = glob.glob('./camera_cal/calibration*.jpg')\n",
    "\n",
    "    for fname in images:\n",
    "        img = cv2.imread(fname)\n",
    "        gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)\n",
    "        ret, corners = cv2.findChessboardCorners(gray, (9,6),None)\n",
    "\n",
    "        # If found, add object points, image points\n",
    "        if ret == True:\n",
    "            objpoints.append(objp)\n",
    "            imgpoints.append(corners)\n",
    "    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_size,None,None)\n",
    "    return mtx, dist\n",
    "\n",
    "#### Apply a distortion correction to raw images. ####\n",
    "\n",
    "def undistort(img):\n",
    "    img_size = (img.shape[1], img.shape[0])\n",
    "    mtx, dist = get_camera_calibration(img_size)\n",
    "    dst = cv2.undistort(img, mtx, dist, None, mtx)\n",
    "    return dst\n",
    "\n",
    "def apply_distortion_correction():\n",
    "    image_names = os.listdir('./test_images')\n",
    "    i = 0\n",
    "    images = glob.glob('./test_images/*.jpg')\n",
    "    \n",
    "    for fname in images:\n",
    "        img = mpimg.imread(fname)\n",
    "        dst = undistort(img)\n",
    "        cv2.imwrite('./output_images/undistorted_images/undist_' + image_names[i] ,dst)\n",
    "        i = i + 1\n",
    "        # plot_all(img, dst, 'Test_image', 'Undistorted Image.')\n",
    "\n",
    "    visualize(\"output_images/grid_images/images_grid_undistord.jpg\",\n",
    "          (mpimg.imread(f) for f in cycle(glob.glob(\"output_images/undistorted_images/undist_*.jpg\"))))\n",
    "\n",
    "\n",
    "# Apply a distortion correction to raw images.\n",
    "apply_distortion_correction()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "def measure_warp(img):\n",
    "    top = 0\n",
    "    bottom = img.shape[0]\n",
    "    def handler(e):\n",
    "        if len(src)<4:\n",
    "            plt.axhline(int(e.ydata), linewidth=2, color='r')\n",
    "            plt.axvline(int(e.xdata), linewidth=2, color='r')\n",
    "            src.append((int(e.xdata),int(e.ydata)))\n",
    "        if len(src)==4:\n",
    "            dst.extend([(100,bottom),(100,top),(1180,top),(1180,bottom)])\n",
    "    was_interactive = matplotlib.is_interactive()\n",
    "    if not matplotlib.is_interactive():\n",
    "        plt.ion()\n",
    "    fig = plt.figure()\n",
    "    plt.imshow(img)\n",
    "    global src                                                            \n",
    "    global dst                                                            \n",
    "    src = []\n",
    "    dst = []\n",
    "    cid1 = fig.canvas.mpl_connect('button_press_event', handler)\n",
    "    cid2 = fig.canvas.mpl_connect('close_event', lambda e: e.canvas.stop_event_loop())\n",
    "    fig.canvas.start_event_loop(timeout=-1)\n",
    "    M = cv2.getPerspectiveTransform((np.asfarray(src, np.float32)), (np.asfarray(dst, np.float32)))\n",
    "    Minv = cv2.getPerspectiveTransform(np.asfarray(dst, np.float32), np.asfarray(src, np.float32))\n",
    "    matplotlib.interactive(was_interactive)\n",
    "    return M, Minv\n",
    "\n",
    "def save_matrix(M, Minv, name):\n",
    "    np.save('warp_matrix_'  + name, M)\n",
    "    np.save('unwarp_matrix_'  + name , Minv)\n",
    "\n",
    "def perspective_transform(img):\n",
    "\n",
    "    img_size = (img.shape[1], img.shape[0])\n",
    "    M, Minv = measure_warp(img)\n",
    "    warped = cv2.warpPerspective(img, M, img_size)\n",
    "    unwarp = cv2.warpPerspective(warped, Minv, img_size)\n",
    "    return warped, unwarp, M, Minv\n",
    "\n",
    "def apply_perspective_transform():\n",
    "    \n",
    "    image_names = os.listdir('./test_images')\n",
    "    i = 0\n",
    "    images = glob.glob('./output_images/undistorted_images/*.jpg')\n",
    "    \n",
    "    for fname in images:\n",
    "        img = mpimg.imread(fname)\n",
    "        warped_image, unwarp, M, Minv = perspective_transform(img)\n",
    "        name = image_names[i][:len(image_names[i]) - 4]\n",
    "        save_matrix(M, Minv, name)\n",
    "        cv2.imwrite('./output_images/warped_images/warped_' + image_names[i] ,warped_image)\n",
    "        cv2.imwrite('./output_images/unwarp_images/unwarped_' + image_names[i] ,unwarp)\n",
    "        i = i + 1\n",
    "        plot_all(img, unwarp,'Undistorted Image.', 'Unwarped Grad.')\n",
    "        plot_all(img, warped_image,'Undistorted Image.', 'Warped Grad.')\n",
    "        \n",
    "    visualize(\"output_images/grid_images/images_grid_warped.jpg\", \n",
    "              (mpimg.imread(f) for f in cycle(glob.glob(\"output_images/warped_images/.jpg\"))))\n",
    "    visualize(\"output_images/grid_images/images_grid_unwarped.jpg\", \n",
    "          (mpimg.imread(f) for f in cycle(glob.glob(\"output_images/unwarp_images/.jpg\"))))\n",
    "    \n",
    "apply_perspective_transform()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "def binary_threshold(img, thresh):\n",
    "    binary = np.zeros_like(img)\n",
    "    binary[(img > thresh[0]) & (img <= thresh[1])] = 1\n",
    "\n",
    "    return binary\n",
    "def abs_sobel_func(img, orient, sobel_kernel = 3):\n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "    sobel = cv2.Sobel(gray, cv2.CV_64F, orient == 'x', orient == 'y', ksize=sobel_kernel)\n",
    "    abs_sobel = np.absolute(sobel)\n",
    "    return abs_sobel\n",
    "\n",
    "def abs_sobel_thresh(img, orient='x', sobel_kernel=3, thresh=(20,100)):\n",
    "    abs_sobel = abs_sobel_func(img, orient, sobel_kernel)\n",
    "    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))\n",
    "    sxbinary = binary_threshold(scaled_sobel, thresh)\n",
    "    return sxbinary\n",
    "\n",
    "def mag_thresh(img, sobel_kernel=9, mag_thresh=(30, 100)):\n",
    "    \n",
    "    abs_sobelx = abs_sobel_func(img, 'x', sobel_kernel)\n",
    "    abs_sobely = abs_sobel_func(img, 'y', sobel_kernel)\n",
    "    abs_sobel = np.sqrt(abs_sobelx**2 + abs_sobely**2)\n",
    "    \n",
    "    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))\n",
    "    mask = binary_threshold(scaled_sobel, mag_thresh)\n",
    "    return mask\n",
    "\n",
    "def dir_threshold(img, sobel_kernel=15, thresh=(0.7, 1.3)):\n",
    "\n",
    "    abs_sobelx = abs_sobel_func(img, 'x', sobel_kernel)\n",
    "    abs_sobely = abs_sobel_func(img, 'y', sobel_kernel)\n",
    "    \n",
    "    # Use np.arctan2(abs_sobely, abs_sobelx) to calculate the direction of the gradient \n",
    "    dir_gradient = np.arctan2(abs_sobely, abs_sobelx)\n",
    "    binary_output = binary_threshold(dir_gradient, thresh)\n",
    "    return binary_output\n",
    "\n",
    "def apply_gradian_threshold(image):\n",
    "    ksize = 3\n",
    "    \n",
    "    gradx = abs_sobel_thresh(image, orient='x',  sobel_kernel=3, thresh=(20, 100))\n",
    "    \n",
    "    grady = abs_sobel_thresh(image, orient='y',  sobel_kernel=3, thresh=(20, 100))\n",
    "    \n",
    "    mag_binary = mag_thresh(image,  sobel_kernel=3, mag_thresh=(70, 100))\n",
    "    \n",
    "    dir_binary = dir_threshold(image,  sobel_kernel=3, thresh=(0.7, 0.9))\n",
    "    \n",
    "    grad_binary = np.zeros_like(dir_binary)\n",
    "#     grad_binary[((gradx == 1)) | ((mag_binary == 1) & (dir_binary == 1))] = 1\n",
    "    grad_binary[(gradx == 1)] = 1\n",
    "\n",
    "    return grad_binary\n",
    "\n",
    "def apply_color_threshold(img):\n",
    "\n",
    "    r_channel = img[:,:,0]\n",
    "    thresh = (150, 255)\n",
    "    r_binary = binary_threshold(r_channel, thresh)\n",
    "    \n",
    "    g_channel = img[:,:,1]\n",
    "    thresh = (200, 255)\n",
    "    g_binary = binary_threshold(g_channel, thresh)\n",
    "    \n",
    "    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)\n",
    "    s_channel = hls[:,:,2]\n",
    "    thresh = (170, 255)\n",
    "    s_binary = binary_threshold(s_channel, thresh)\n",
    "    \n",
    "    yuv = cv2.cvtColor(img, cv2.COLOR_RGB2YUV)\n",
    "    u_channel = yuv[:,:,1]\n",
    "    thresh = (0, 0)\n",
    "    u_binary = binary_threshold(u_channel, thresh)\n",
    "    \n",
    "    color_binary = np.zeros_like(s_binary)\n",
    "#     color_binary[(s_binary == 1) | (u_binary == 1) | ((r_binary == 1) & (g_binary == 1))] = 1\n",
    "    color_binary[(s_binary == 1) ] = 1\n",
    "    return s_binary\n",
    "\n",
    "def get_binary_image(img):\n",
    "    gradient_binary = apply_gradian_threshold(img)\n",
    "    color_binary = apply_color_threshold(img)\n",
    "\n",
    "    combined_binary = np.zeros_like(gradient_binary)\n",
    "    combined_binary[(gradient_binary == 1) | (color_binary == 1)] = 1\n",
    "    return combined_binary\n",
    "    \n",
    "def save_binary_images():\n",
    "    image_names = os.listdir('./test_images')\n",
    "    i = 0\n",
    "    images = glob.glob('./output_images/warped_images/*.jpg')\n",
    "    \n",
    "    for fname in images:\n",
    "        img = mpimg.imread(fname)\n",
    "        combined_binary = get_binary_image(img)\n",
    "        \n",
    "        cv2.imwrite('./output_images/binary_images/binary_' + image_names[i] ,combined_binary)\n",
    "        i = i + 1\n",
    "        plot_all(img, combined_binary,'Warped Image.', 'Thresholded Grad.')\n",
    "        \n",
    "save_binary_images()        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "visualize(\"output_images/grid_images/images_grid_binary.jpg\",\n",
    "          (mpimg.imread(f) for f in cycle(glob.glob(\"output_images/binary_images/*.jpg\"))))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "def detect_lines_sliding_window(warped_binary):\n",
    "    # Assuming you have created a warped binary image called \"warped_binary\"\n",
    "    # Take a histogram of the bottom half of the image\n",
    "    histogram = np.sum(warped_binary[warped_binary.shape[0]/2:,:], axis=0)\n",
    "    # Create an output image to draw on and  visualize the result\n",
    "    out_img = np.dstack((warped_binary, warped_binary, warped_binary))*255\n",
    "#     print(warped_binary.shape)\n",
    "#     print(out_img.shape)\n",
    "    \n",
    "    # Find the peak of the left and right halves of the histogram\n",
    "    # These will be the starting point for the left and right lines\n",
    "    midpoint = np.int(histogram.shape[0]/2)\n",
    "    leftx_base = np.argmax(histogram[:midpoint])\n",
    "    rightx_base = np.argmax(histogram[midpoint:]) + midpoint\n",
    "    # Choose the number of sliding windows\n",
    "    nwindows = 9\n",
    "    # Set height of windows\n",
    "    window_height = np.int(warped_binary.shape[0]/nwindows)\n",
    "    # Identify the x and y positions of all nonzero pixels in the image\n",
    "    nonzero = warped_binary.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",
    "    # 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_binary.shape[0] - (window+1)*window_height\n",
    "        win_y_high = warped_binary.shape[0] - window*window_height\n",
    "        win_xleft_low = leftx_current - margin\n",
    "        win_xleft_high = leftx_current + margin\n",
    "        win_xright_low = rightx_current - margin\n",
    "        win_xright_high = rightx_current + margin\n",
    "        # Draw the windows on the visualization image\n",
    "        cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),(0,255,0), 2) \n",
    "        cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 2) \n",
    "        # Identify the nonzero pixels in x and y within the window\n",
    "        good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]\n",
    "        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (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",
    "    # 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",
    "    # Fit a second order polynomial to each\n",
    "#     left_fit,left_res,_,_,_ = np.polyfit(lefty, leftx, 2, full=True)\n",
    "#     right_fit,right_res,_,_,_ = np.polyfit(righty, rightx, 2, full=True)\n",
    "    \n",
    "    left_fit = np.polyfit(lefty, leftx, 2)\n",
    "    right_fit = np.polyfit(righty, rightx, 2)\n",
    "    # Generate x and y values for plotting\n",
    "#     ploty = np.linspace(0, warped_binary.shape[0]-1, warped_binary.shape[0] )\n",
    "    ploty = np.linspace(0, 719, 720)\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",
    "#     print(len(left_fitx))\n",
    "#     print(len(right_fitx))\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",
    "#     out_img[ploty.astype('int'),left_fitx.astype('int')] = [0, 255, 255]\n",
    "#     out_img[ploty.astype('int'),right_fitx.astype('int')] = [0, 255, 255]\n",
    "    y_eval = warped_binary.shape[0]\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 new polynomials to x,y in world space\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",
    "    # Calculate the new radii of curvature\n",
    "#     left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + 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 + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])\n",
    "#     return left_fit, right_fit, np.sqrt(left_fit[1]/len(leftx)), np.sqrt(right_fit[1]/len(rightx)), left_curverad, right_curverad, out_img\n",
    "    return left_fit, right_fit, np.sqrt(left_fit[1]/len(leftx)), np.sqrt(right_fit[1]/len(rightx)),out_img\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/shokoofeh/.local/lib/python2.7/site-packages/ipykernel_launcher.py:92: RuntimeWarning: invalid value encountered in sqrt\n"
     ]
    }
   ],
   "source": [
    "\n",
    "def window_mask(width, height, img_ref, center,level):\n",
    "    output = np.zeros_like(img_ref)\n",
    "    output[int(img_ref.shape[0]-(level+1)*height):int(img_ref.shape[0]-level*height),max(0,int(center-width/2)):min(int(center+width/2),img_ref.shape[1])] = 1\n",
    "    return output\n",
    "\n",
    "def find_window_centroids(image, window_width, window_height, margin):\n",
    "    \n",
    "    window_centroids_r = [] # Store the (left,right) window centroid positions per level\n",
    "    window_centroids_l = [] # Store the (left,right) window centroid positions per level\n",
    "    window = np.ones(window_width) # Create our window template that we will use for convolutions\n",
    "    ypoint_r = []\n",
    "    ypoint_l = []\n",
    "    \n",
    "    # Sum quarter bottom of image to get slice, could use a different ratio\n",
    "    l_sum = np.sum(image[int(3*image.shape[0]/4):,:int(image.shape[1]/2)], axis=0)\n",
    "    l_center = np.argmax(np.convolve(window,l_sum))-window_width/2\n",
    "    r_sum = np.sum(image[int(3*image.shape[0]/4):,int(image.shape[1]/2):], axis=0)\n",
    "    r_center = np.argmax(np.convolve(window,r_sum))-window_width/2+int(image.shape[1]/2)\n",
    "    \n",
    "    # Add what we found for the first layer\n",
    "    window_centroids_r.append(r_center)\n",
    "    window_centroids_l.append(l_center)\n",
    "    ymax = image.shape[0]\n",
    "    ymin = int(image.shape[0]-window_height)\n",
    "    ymid = int((ymax + ymin) / 2)\n",
    "    ypoint_r.append(ymid)\n",
    "    ypoint_l.append(ymid)\n",
    "    # Go through each layer looking for max pixel locations\n",
    "    for level in range(1,(int)(image.shape[0]/window_height)):\n",
    "        # convolve the window into the vertical slice of the image\n",
    "        \n",
    "        image_layer = np.sum(image[int(image.shape[0]-(level+1)*window_height):int(image.shape[0]-level*window_height),:], axis=0)\n",
    "        conv_signal = np.convolve(window, image_layer)\n",
    "        ymax = int(image.shape[0]-level*window_height)\n",
    "        ymin = int(image.shape[0]-(level+1)*window_height)\n",
    "        ymid = int((ymax + ymin) / 2)\n",
    "#         ypoint.append(ymid)\n",
    "        # Find the best left centroid by using past left center as a reference\n",
    "        # Use window_width/2 as offset because convolution signal reference is at right side of window, not center of window\n",
    "        offset = window_width/2\n",
    "        \n",
    "        l_min_index = int(max(l_center+offset-margin,0))\n",
    "        l_max_index = int(min(l_center+offset+margin,image.shape[1]))\n",
    "        l_center = np.argmax(conv_signal[l_min_index:l_max_index])+l_min_index-offset\n",
    "        if(l_center > l_min_index-offset):\n",
    "            window_centroids_l.append(l_center)\n",
    "            ypoint_l.append(ymid)\n",
    "            \n",
    "            \n",
    "        # Find the best right centroid by using past right center as a reference\n",
    "        r_min_index = int(max(r_center+offset-margin,0))\n",
    "        r_max_index = int(min(r_center+offset+margin,image.shape[1]))\n",
    "        r_center = np.argmax(conv_signal[r_min_index:r_max_index])+r_min_index-offset\n",
    "        # Add what we found for that layer\n",
    "        if(r_center > r_min_index-offset):\n",
    "            window_centroids_r.append(r_center)\n",
    "            ypoint_r.append(ymid)\n",
    "    # return window_centroids\n",
    "    return window_centroids_l, ypoint_l, window_centroids_r, ypoint_r\n",
    "\n",
    "def polyfit(window_centroids, ypoint):\n",
    "    ploty = np.array(ypoint)\n",
    "    xp = np.array(window_centroids)\n",
    "    polyfit = np.polyfit(ploty, xp, 2)\n",
    "    #polyfitx = polyfit[0]*ploty**2 + polyfit[1]*ploty + polyfit[2]\n",
    "    return polyfit\n",
    "\n",
    "def measure_polyfit(window_centroids, ypoint):\n",
    "    ploty = np.array(ypoint)\n",
    "    xp = np.array(window_centroids)\n",
    "    polyfit = np.polyfit(ploty, xp, 2)\n",
    "    #polyfitx = polyfit[0]*ploty**2 + polyfit[1]*ploty + polyfit[2]\n",
    "    return polyfit\n",
    "    \n",
    "\n",
    "def measure_curvature(polyfit, ypoint):\n",
    "    ploty = np.linspace(0, 719, num=720)\n",
    "    A = polyfit[0]\n",
    "    B = polyfit[1]\n",
    "    y_eval = np.max(ploty)\n",
    "    curvature = (1 + (2 * A * y_eval + B) ** 2) ** 1.5 / (2 * np.absolute(A))\n",
    "    return curvature\n",
    "\n",
    "def measure_curvature_real(polyfit, ypoint):  \n",
    "    ym_per_pix = 3./300 # meters per pixel in y dimension\n",
    "    xm_per_pix = 3.7/400 # meters per pixel in x dimension\n",
    "    \n",
    "    left_fit_cr = np.polyfit(ploty_l * ym_per_pix, leftx * xm_per_pix, 2)\n",
    "    right_fit_cr = np.polyfit(ploty_r * ym_per_pix, rightx* xm_per_pix , 2)\n",
    "    \n",
    "    left_curverad = ((1 + (2*left_fit_cr[0]*y_eval_l*ym_per_pix + 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_r*ym_per_pix + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])\n",
    "    return left_curverad, right_curverad\n",
    "\n",
    "def polyfits(warped_img):\n",
    "    window_width = 50 \n",
    "    window_height = 80 # Break image into 9 vertical layers since image height is 720\n",
    "    margin = 100 # How much to slide left and right for searching\n",
    "\n",
    "    window_centroids_l, ypoint_l, window_centroids_r, ypoint_r = find_window_centroids(warped_img, window_width, window_height, margin)\n",
    "    \n",
    "    left_polyfit = measure_polyfit(window_centroids_l, ypoint_l)\n",
    "    right_polyfit = measure_polyfit(window_centroids_r, ypoint_r)\n",
    "    \n",
    "    ploty = np.linspace(0, 719, num=720)# to cover same y-range as imag\n",
    "\n",
    "    left_fitx = left_polyfit[0]*ploty**2 + left_polyfit[1]*ploty + left_polyfit[2]\n",
    "    right_fitx = right_polyfit[0]*ploty**2 + right_polyfit[1]*ploty + right_polyfit[2]\n",
    "    \n",
    "#         # Plot up the fake data\n",
    "    mark_size = 3\n",
    "    plt.xlim(0, 1280)\n",
    "    plt.ylim(0, 720)\n",
    "    plt.plot(left_fitx, ploty, color='green', linewidth=3)\n",
    "    plt.plot(right_fitx, ploty, color='green', linewidth=3)\n",
    "    plt.gca().invert_yaxis() # to visualize as we do the images\n",
    "    return left_polyfit, right_polyfit\n",
    "    \n",
    "    \n",
    "def draw_lane(undistorted, warped_binary,newwarp, left_fit, right_fit, Minv):\n",
    "    # Create an image to draw the lines on\n",
    "    warp_zero = np.zeros_like(warped_binary).astype(np.uint8)\n",
    "    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))\n",
    "    nonzero = warped_binary.nonzero()\n",
    "    nonzeroy = np.array(nonzero[0])\n",
    "    nonzerox = np.array(nonzero[1])\n",
    "    \n",
    "    # Generate x and y values for plotting\n",
    "    ploty = np.linspace(0, warped_binary.shape[0]-1, warped_binary.shape[0])\n",
    "    left_fitx = left_fit[0]*nonzeroy**2 + left_fit[1]*nonzeroy + left_fit[2]\n",
    "    right_fitx = right_fit[0]*nonzeroy**2 + right_fit[1]*nonzeroy + right_fit[2]\n",
    "    \n",
    "    margin = 50\n",
    "    left_lane_inds = ((left_fitx - margin < nonzerox) & (nonzerox < left_fitx + margin))\n",
    "    right_lane_inds = ((right_fitx - margin < nonzerox) & (nonzerox < right_fitx + margin))\n",
    "    \n",
    "        ## Visualization ##\n",
    "    # Create an image to draw on and an image to show the selection window\n",
    "    out_img = np.dstack((warped_binary, warped_binary, warped_binary))*255\n",
    "    window_img = np.zeros_like(out_img)\n",
    "    # Color in left and right line pixels\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",
    "    left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, nonzeroy]))])\n",
    "    left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, \n",
    "                              nonzeroy])))])\n",
    "    left_line_pts = np.hstack((left_line_window1, left_line_window2))\n",
    "    right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, nonzeroy]))])\n",
    "    right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, \n",
    "                              nonzeroy])))])\n",
    "    right_line_pts = np.hstack((right_line_window1, right_line_window2))\n",
    "    \n",
    "    cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))\n",
    "    cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))\n",
    "    result_line = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)\n",
    "\n",
    "    # Draw the lane onto the warped_binary blank image\n",
    "    pts = np.hstack((np.array([np.flipud(np.transpose(np.vstack([left_fitx, \n",
    "                              nonzeroy])))]), np.array([np.transpose(np.vstack([right_fitx, nonzeroy]))])))\n",
    "    cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))\n",
    "    newwarp = cv2.warpPerspective(color_warp, Minv, (undistorted.shape[1], undistorted.shape[0])) \n",
    "    result_lane = cv2.addWeighted(undistorted, 1, newwarp, 0.6, 0)\n",
    "    y_eval = warped_binary.shape[0]\n",
    "    ym_per_pix = 30.0/720 # meters per pixel in y dimension\n",
    "    xm_per_pix = 3.7/700 # meters per pixel in x dimension\n",
    "    # Fit new polynomials to x,y in world space\n",
    "    left_fit_cr = np.polyfit(nonzeroy*ym_per_pix, left_fitx*xm_per_pix, 2)\n",
    "    right_fit_cr = np.polyfit(nonzeroy*ym_per_pix, right_fitx*xm_per_pix, 2)\n",
    "    # Calculate the new radii of curvature\n",
    "    left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + 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 + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])\n",
    "    cv2.putText(result_lane, \"L. Curvature: %.2f km\" % (left_curverad/1000), (50,50), cv2.FONT_HERSHEY_DUPLEX, 1, (255,255,255), 2)\n",
    "    cv2.putText(result_lane, \"R. Curvature: %.2f km\" % (right_curverad/1000), (50,80), cv2.FONT_HERSHEY_DUPLEX, 1, (255,255,255), 2)\n",
    "    # Annotate image with position estimate\n",
    "#     cv2.putText(result, \"C. Position: %.2f m\" % ((np.average((l_fitx + r_fitx)/2) - warped_binary.shape[1]//2)*3.7/700), (50,110), cv2.FONT_HERSHEY_DUPLEX, 1, (255,255,255), 2)\n",
    "    \n",
    "    return result_lane, result_line, newwarp, color_warp\n",
    "\n",
    "# save warped image\n",
    "def save_lane_images():\n",
    "    \n",
    "    image_names = os.listdir('./test_images')\n",
    "    \n",
    "    binary_images = glob.glob('./output_images/binary_images/*.jpg')\n",
    "    undistorted_images = glob.glob('./output_images/undistorted_images/*.jpg')\n",
    "    unwarped_images = glob.glob('./output_images/unwarp_images/*.jpg')\n",
    "    \n",
    "    for i in range(len(image_names)):\n",
    "        warped_binary = cv2.imread(binary_images[i], cv2.IMREAD_GRAYSCALE)\n",
    "        undistorted = cv2.imread(undistorted_images[i])\n",
    "        unwarp = cv2.imread(unwarped_images[i])\n",
    "        l_fit, r_fit, l_res, r_res, _ = detect_lines_sliding_window(warped_binary)\n",
    "        \n",
    "        name = image_names[i][:len(image_names[i]) - 4]\n",
    "        Minv = np.load('unwarp_matrix_' + name + '.npy' )\n",
    "        \n",
    "        lane_image, line_image, newwarp, color_warp = draw_lane(undistorted, warped_binary,unwarp, l_fit, r_fit, Minv)\n",
    "        cv2.imwrite('./output_images/lane_images/lane_' + image_names[i] ,lane_image)\n",
    "        cv2.imwrite('./output_images/line_images/line_' + image_names[i] ,line_image)\n",
    "        plot_all(line_image, lane_image,'warped_binary.', 'lane_image.')\n",
    "\n",
    "save_lane_images()\n",
    "visualize(\"output_images/grid_images/images_grid_lane.jpg\",\n",
    "          (mpimg.imread(f) for f in cycle(glob.glob(\"output_images/lane_images/*.jpg\"))))\n",
    "visualize(\"output_images/grid_images/images_grid_line.jpg\",\n",
    "          (mpimg.imread(f) for f in cycle(glob.glob(\"output_images/line_images/*.jpg\"))))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}