p4.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": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Using matplotlib backend: MacOSX\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import glob\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.image as mpimg\n",
    "#%matplotlib qt\n",
    "%matplotlib"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def calibrate():\n",
    "    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)\n",
    "    objp = np.zeros((6*9,3), np.float32)\n",
    "    objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)\n",
    "\n",
    "    # Arrays to store object points and image points from all the images.\n",
    "    objpoints = [] # 3d points in real world space\n",
    "    imgpoints = [] # 2d points in image plane.\n",
    "\n",
    "    # Make a list of calibration images\n",
    "    images = glob.glob('./camera_cal/calibration*.jpg')\n",
    "\n",
    "    # Step through the list and search for chessboard corners\n",
    "    for fname in images:\n",
    "        img = cv2.imread(fname)\n",
    "        gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)\n",
    "\n",
    "        # Find the chessboard corners\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",
    "\n",
    "            # Draw and display the corners\n",
    "            img = cv2.drawChessboardCorners(img, (9,6), corners, ret)\n",
    "            cv2.imshow('img',img)\n",
    "            cv2.waitKey(500)\n",
    "\n",
    "    cv2.destroyAllWindows()\n",
    "    \n",
    "    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)\n",
    "    return ret, mtx, dist, rvecs, tvecs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "ret, mtx, dist, rvecs, tvecs = calibrate()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Edit this function to create your own pipeline.\n",
    "def do_threshold(img, s_thresh=(170, 255), sx_thresh=(20, 100)):\n",
    "    img = np.copy(img)    \n",
    "\n",
    "    # Convert to HLS color space and separate the V channel\n",
    "    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS).astype(np.float)\n",
    "    h_channel = hls[:,:,0]\n",
    "    l_channel = hls[:,:,1]\n",
    "    s_channel = hls[:,:,2]\n",
    "    \n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "    \n",
    "    # Sobel x\n",
    "    sobelx = cv2.Sobel(l_channel, cv2.CV_64F, 1, 0) # Take the derivative in x\n",
    "    abs_sobelx = np.absolute(sobelx) # Absolute x derivative to accentuate lines away from horizontal\n",
    "    scaled_sobel = np.uint8(255*abs_sobelx/np.max(abs_sobelx))\n",
    "    \n",
    "    # Threshold x gradient\n",
    "    sxbinary = np.zeros_like(scaled_sobel)\n",
    "    sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])] = 1\n",
    "    \n",
    "    # Threshold color channel\n",
    "    s_binary = np.zeros_like(s_channel)\n",
    "    s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1\n",
    "    # Stack each channel\n",
    "    # Note color_binary[:, :, 0] is all 0s, effectively an all black image. It might\n",
    "    # be beneficial to replace this channel with something else.\n",
    "    color_binary = np.dstack(( np.zeros_like(sxbinary), sxbinary, s_binary))\n",
    "    \n",
    "    thresh = (10, 130)\n",
    "    h_binary = np.zeros_like(h_channel)\n",
    "    h_binary[(h_channel > thresh[0]) & (h_channel <= thresh[1])] = 1\n",
    "    \n",
    "    combined = np.zeros_like(s_binary)\n",
    "    #combined[((sxbinary == 1) | ((s_binary == 1) & (h_binary == 1)))] = 1  \n",
    "    combined[((sxbinary == 1) | ((s_binary == 1)))] = 1  \n",
    "    \n",
    "    return combined"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def transform(img):\n",
    "    # from straight_lines2.jpg   \n",
    "    src = np.float32([\n",
    "        [265,683], [1060,683], [738,480], [552, 480]\n",
    "    ])\n",
    "    dst = np.float32([\n",
    "        [300,693], [980,693],[980,350], [300, 350]\n",
    "    ])\n",
    "    \n",
    "    \"\"\" \n",
    "    src = np.float32([\n",
    "        [585, 460], [203, 720], [1127, 720], [695, 460]\n",
    "    ])\n",
    "    dst = np.float32([\n",
    "        [320, 0], [320, 720],[960, 720], [960, 0]\n",
    "    ])\n",
    "    \"\"\"\n",
    "    \"\"\"\n",
    "    img_size = img.shape\n",
    "    src = np.float32(\n",
    "        [[(img_size[0] / 2) - 55, img_size[1] / 2 + 100],\n",
    "        [((img_size[0] / 6) - 10), img_size[1]],\n",
    "        [(img_size[0] * 5 / 6) + 60, img_size[1]],\n",
    "        [(img_size[0] / 2 + 55), img_size[1] / 2 + 100]])\n",
    "    dst = np.float32(\n",
    "        [[(img_size[0] / 4), 0],\n",
    "        [(img_size[0] / 4), img_size[1]],\n",
    "        [(img_size[0] * 3 / 4), img_size[1]],\n",
    "        [(img_size[0] * 3 / 4), 0]])\n",
    "    \n",
    "    \n",
    "    src = np.float32([\n",
    "        [265,683], [1060,683], [738,480], [552, 480]\n",
    "    ])\n",
    "    dst = np.float32([\n",
    "        [210,693], [1100,693],[1100,350], [210, 350]\n",
    "    ])\n",
    "    \n",
    "    dst = np.float32([\n",
    "        [210,693], [1100,693],[1100,350], [210, 350]\n",
    "    ])\n",
    "    dst = np.float32([\n",
    "        [250,683], [1075,683],[1075,1], [250, 1]\n",
    "    ])\n",
    "     \n",
    "    src = np.float32([\n",
    "        [265,683], [1060,683], [883,572], [421, 572]\n",
    "    ])\n",
    "    dst = np.float32([\n",
    "        [265,683], [1060,683],[1060,500], [265, 500]\n",
    "    ])\n",
    "    \"\"\"\n",
    "    \"\"\" \n",
    "    # from straight_lines1.jpg\n",
    "    src = np.float32([\n",
    "        [454,548], [835,548], [1011,661], [300, 661]\n",
    "    ])\n",
    "    dst = np.float32([\n",
    "        [300,450], [1011,450],[1011,661], [300, 661]\n",
    "    ])\n",
    "    \"\"\"\n",
    "    M = cv2.getPerspectiveTransform(src, dst)\n",
    "    Minv = cv2.getPerspectiveTransform(dst, src)\n",
    "    img_size = (img.shape[1], img.shape[0])\n",
    "    warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)\n",
    "    \n",
    "    return (warped, Minv)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "def getLeftRightPoly(binary_warped):\n",
    "    histogram = np.sum(binary_warped[int(binary_warped.shape[0]/2):,:], axis=0)\n",
    "    # Create an output image to draw on and  visualize the result\n",
    "    out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255\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",
    "\n",
    "    # Choose the number of sliding windows\n",
    "    nwindows = 9\n",
    "    # Set height of windows\n",
    "    window_height = np.int(binary_warped.shape[0]/nwindows)\n",
    "    # Identify the x and y positions of all nonzero pixels in the image\n",
    "    nonzero = binary_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",
    "\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 = binary_warped.shape[0] - (window+1)*window_height\n",
    "        win_y_high = binary_warped.shape[0] - window*window_height\n",
    "        win_xleft_low = leftx_current - margin\n",
    "        win_xleft_high = leftx_current + margin\n",
    "        win_xright_low = rightx_current - margin\n",
    "        win_xright_high = rightx_current + margin\n",
    "        # Draw the windows on the visualization image\n",
    "        cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),\n",
    "        (0,255,0), 2) \n",
    "        cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),\n",
    "        (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) & \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",
    "\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",
    "\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",
    "    # Fit a second order polynomial to each\n",
    "    left_fit = np.polyfit(lefty, leftx, 2)\n",
    "    right_fit = np.polyfit(righty, rightx, 2)\n",
    "    \n",
    "    # Generate x and y values for plotting\n",
    "    ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.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",
    "    \n",
    "    # save left_fit and right_fit to Line objects\n",
    "    leftLine.allx = leftx\n",
    "    leftLine.ally = lefty  \n",
    "    leftLine.current_fit = left_fit\n",
    "    leftLine.best_fit = left_fit\n",
    "    \n",
    "    rightLine.allx = rightx\n",
    "    rightLine.ally = righty\n",
    "    rightLine.current_fit = right_fit\n",
    "    rightLine.best_fit = right_fit\n",
    "    \n",
    "    return (ploty, left_fitx, right_fitx)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def getLeftRightPolyUsePrevious(binary_warped):\n",
    "    # Assume you now have a new warped binary image \n",
    "    # from the next frame of video (also called \"binary_warped\")\n",
    "    # It's now much easier to find line pixels!\n",
    "    nonzero = binary_warped.nonzero()\n",
    "    nonzeroy = np.array(nonzero[0])\n",
    "    nonzerox = np.array(nonzero[1])\n",
    "    margin = 100\n",
    "    \n",
    "    # get fit from previous and Fit a second order polynomial to each\n",
    "    leftx = leftLine.allx \n",
    "    lefty = leftLine.ally     \n",
    "    rightx = rightLine.allx \n",
    "    righty = rightLine.ally\n",
    "    left_fit = np.polyfit(lefty, leftx, 2)\n",
    "    right_fit = np.polyfit(righty, rightx, 2)\n",
    "    \n",
    "    left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + \n",
    "    left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) + \n",
    "    left_fit[1]*nonzeroy + left_fit[2] + margin))) \n",
    "\n",
    "    right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + \n",
    "    right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) + \n",
    "    right_fit[1]*nonzeroy + right_fit[2] + margin)))  \n",
    "\n",
    "    # Again, 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 = np.polyfit(lefty, leftx, 2)\n",
    "    right_fit = np.polyfit(righty, rightx, 2)\n",
    "    \n",
    "    # check if the highest order of fit changes positive <-> negative, if so means bad\n",
    "    # but if two fit changes together, then still ok\n",
    "    # if only one changes, then ignore that one\n",
    "    left_part = 0.2\n",
    "    right_part = 0.2\n",
    "    if ((left_fit[0] * leftLine.best_fit[0] > 0) & (right_fit[0] * rightLine.best_fit[0] < 0)):\n",
    "        # left line ok, right line bad, ignore right_fit, use previous fit\n",
    "        #print(\"right fit bad\")\n",
    "        right_part = right_part / 4\n",
    "    elif ((left_fit[0] * leftLine.best_fit[0] < 0) & (right_fit[0] * rightLine.best_fit[0] > 0)):\n",
    "        # left line bad, right line ok\n",
    "        #print(\"left fit bad\")\n",
    "        left_part = left_part / 4\n",
    "        \n",
    "    # try moving average of 5 iterations\n",
    "    #print(\"left_fit: \", left_fit, \"right: \", right_fit)\n",
    "    left_fit = left_fit * left_part + leftLine.best_fit * (1 - left_part)\n",
    "    right_fit = right_fit * right_part + rightLine.best_fit * (1 - right_part)\n",
    "    #print(\"avg left_fit: \", left_fit, \"right: \", right_fit)\n",
    "    \n",
    "    # Generate x and y values for plotting\n",
    "    ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.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",
    "    \n",
    "    # save left_fit and right_fit to Line objects\n",
    "    leftLine.allx = leftx\n",
    "    leftLine.ally = lefty  \n",
    "    leftLine.current_fit = left_fit\n",
    "    leftLine.best_fit = left_fit\n",
    "    \n",
    "    rightLine.allx = rightx\n",
    "    rightLine.ally = righty\n",
    "    rightLine.current_fit = right_fit\n",
    "    rightLine.best_fit = right_fit\n",
    "    \n",
    "    return (ploty, left_fitx, right_fitx)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Assuming you have created a warped binary image called \"binary_warped\"\n",
    "# Take a histogram of the bottom half of the image\n",
    "def findLane(binary_warped, Minv):\n",
    "    if (leftLine.allx is None):\n",
    "        #print(\"firt fit\")\n",
    "        ploty, left_fitx, right_fitx = getLeftRightPoly(binary_warped) \n",
    "    else:\n",
    "        #print(\"use previous\")\n",
    "        ploty, left_fitx, right_fitx = getLeftRightPolyUsePrevious(binary_warped)\n",
    "        #ploty, left_fitx, right_fitx = getLeftRightPoly(binary_warped) \n",
    "        \n",
    "    # Define y-value where we want radius of curvature\n",
    "    # I'll choose the maximum y-value, corresponding to the bottom of the image\n",
    "    y_eval = np.max(ploty)\n",
    "    left_curverad = ((1 + (2*left_fitx[0]*y_eval + left_fitx[1])**2)**1.5) / np.absolute(2*left_fitx[0])\n",
    "    right_curverad = ((1 + (2*right_fitx[0]*y_eval + right_fitx[1])**2)**1.5) / np.absolute(2*right_fitx[0])\n",
    "    #print(\"curv\", left_curverad, right_curverad)\n",
    "    # Example values: 1926.74 1908.48 \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",
    "\n",
    "    # Fit new polynomials to x,y in world space\n",
    "    left_fit_cr = np.polyfit(ploty*ym_per_pix, left_fitx*xm_per_pix, 2)\n",
    "    right_fit_cr = np.polyfit(ploty*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",
    "    # Now our radius of curvature is in meters\n",
    "    # print(left_curverad, 'm', right_curverad, 'm')\n",
    "    # Example values: 632.1 m    626.2 m\n",
    "    \n",
    "    \n",
    "    # Create an image to draw the lines on\n",
    "    warp_zero = np.zeros_like(binary_warped).astype(np.uint8)\n",
    "    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))\n",
    "\n",
    "    # Recast the x and y points into usable format for cv2.fillPoly()\n",
    "    pts_left = np.array([np.transpose(np.vstack([left_fitx, 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",
    "\n",
    "    # Warp the blank back to original image space using inverse perspective matrix (Minv)\n",
    "    newwarp = cv2.warpPerspective(color_warp, Minv, (binary_warped.shape[1], binary_warped.shape[0])) \n",
    "    # Combine the result with the original image\n",
    "    \n",
    "    left_p = left_fitx[-1]\n",
    "    right_p = right_fitx[-1]\n",
    "    mid_p = (left_p + right_p) / 2\n",
    "    offset = (mid_p - 640) * xm_per_pix\n",
    "    #print(\"points \", left_p, right_p)\n",
    "    \n",
    "    font = cv2.FONT_HERSHEY_SIMPLEX\n",
    "    text = \"curv left: \" + str(int(left_curverad)) + \", right: \" + str(int(right_curverad))\n",
    "    cv2.putText(newwarp,text,(100,50), font, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "    if (offset > 0):\n",
    "        text = \"offset: \" + \"{:1.2f} m right of center\".format(offset)\n",
    "    else:\n",
    "        text = \"offset: \" + \"{:1.2f} m left of center\".format(abs(offset))\n",
    "    cv2.putText(newwarp,text,(100,100), font, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "    \"\"\" \"\"\"\n",
    "    text = \"left fit: \" + str(leftLine.best_fit) \n",
    "    cv2.putText(newwarp,text,(100,150), font, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "    text = \"right fit: \" + str(rightLine.best_fit)\n",
    "    cv2.putText(newwarp,text,(100,200), font, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "    \n",
    "    \"\"\"  \n",
    "    plt.plot(left_fitx, ploty, color='red')\n",
    "    plt.plot(right_fitx, ploty, color='red')\n",
    "    plt.xlim(0, 1280)\n",
    "    plt.ylim(720, 0)\n",
    "    \"\"\"\n",
    "    \n",
    "    \n",
    "    return newwarp\n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def pipeline(image):\n",
    "    undist = cv2.undistort(image, mtx, dist, None, mtx)\n",
    "    binary = do_threshold(undist, s_thresh=(100, 255), sx_thresh=(20, 100))\n",
    "    warped, Minv = transform(binary)\n",
    "    newwarp = findLane(warped, Minv)\n",
    "    final = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)\n",
    "    return final\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import imageio\n",
    "imageio.plugins.ffmpeg.download()\n",
    "from moviepy.editor import *\n",
    "\n",
    "from moviepy.editor import VideoFileClip\n",
    "from IPython.display import HTML"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Define a class to receive the characteristics of each line detection\n",
    "class Line():\n",
    "    def __init__(self):\n",
    "        # was the line detected in the last iteration?\n",
    "        self.detected = False  \n",
    "        # x values of the last n fits of the line\n",
    "        self.recent_xfitted = [] \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\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": 18,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "leftLine = Line()\n",
    "rightLine = Line()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[MoviePy] >>>> Building video project_output.mp4\n",
      "[MoviePy] Writing video project_output.mp4\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|█████████▉| 1260/1261 [02:53<00:00,  7.34it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[MoviePy] Done.\n",
      "[MoviePy] >>>> Video ready: project_output.mp4 \n",
      "\n",
      "CPU times: user 2min 53s, sys: 53.2 s, total: 3min 46s\n",
      "Wall time: 2min 54s\n"
     ]
    }
   ],
   "source": [
    "leftLine = Line()\n",
    "rightLine = Line()\n",
    "\n",
    "white_output = 'project_output.mp4'\n",
    "## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video\n",
    "## To do so add .subclip(start_second,end_second) to the end of the line below\n",
    "## Where start_second and end_second are integer values representing the start and end of the subclip\n",
    "## You may also uncomment the following line for a subclip of the first 5 seconds\n",
    "#clip1 = VideoFileClip(\"test_videos/solidWhiteRight.mp4\").subclip(0,5)\n",
    "clip1 = VideoFileClip(\"./project_video.mp4\") #.subclip(37,40)\n",
    "white_clip = clip1.fl_image(pipeline) #NOTE: this function expects color images!!\n",
    "%time white_clip.write_videofile(white_output, audio=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "<video width=\"960\" height=\"540\" controls>\n",
       "  <source src=\"project_output.mp4\">\n",
       "</video>\n"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "HTML(\"\"\"\n",
    "<video width=\"960\" height=\"540\" controls>\n",
    "  <source src=\"{0}\">\n",
    "</video>\n",
    "\"\"\".format(white_output))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "leftLine = Line()\n",
    "rightLine = Line()\n",
    "\n",
    "f, (ax1, ax2, ax3) = plt.subplots(1, 3, figsize=(24, 9))\n",
    "f.tight_layout()\n",
    "\n",
    "#image = mpimg.imread('./test_images/straight_lines2.jpg')\n",
    "image = mpimg.imread('./test_images/test6.jpg')\n",
    "#image = cv2.imread('frame1047.jpg')\n",
    "\n",
    "#final = pipeline(image)\n",
    "\n",
    "undist = cv2.undistort(image, mtx, dist, None, mtx)\n",
    "binary = do_threshold(undist, s_thresh=(130, 255), sx_thresh=(20, 100))\n",
    "warped, Minv = transform(binary)\n",
    "newwarp = findLane(warped, Minv)\n",
    "final = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)\n",
    "\n",
    "#undist = cv2.undistort(image, mtx, dist, None, mtx)\n",
    "\n",
    "#binary = do_threshold(undist, s_thresh=(170, 255), sx_thresh=(20, 100))\n",
    "\"\"\" \n",
    "warped, Minv = transform(binary)\n",
    "\n",
    "final = findLane(warped, Minv)\n",
    "\"\"\"\n",
    "#image = mpimg.imread('./test_images/test1.jpg')\n",
    "\"\"\" \"\"\"\n",
    "\n",
    "ax1.imshow(image,cmap='gray')\n",
    "ax1.set_title('Original Image', fontsize=30)\n",
    "ax2.imshow(binary,cmap='gray')\n",
    "ax2.set_title('Binary Image', fontsize=30)\n",
    "ax3.imshow(final,cmap='gray')\n",
    "ax3.set_title('With Line', fontsize=30)\n",
    "#ax3.imshow(final,cmap='gray')\n",
    "\n",
    "plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 752,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "left_fit [  1.33008505e-04  -4.02255679e-01   5.43194142e+02]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "Text(0.5,1,'Poly Image')"
      ]
     },
     "execution_count": 752,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "leftLine = Line()\n",
    "rightLine = Line()\n",
    "\n",
    "\n",
    "\n",
    "#image = mpimg.imread('./camera_cal/calibration2.jpg')\n",
    "image = mpimg.imread('./test_images/test6.jpg')\n",
    "#image = mpimg.imread('frame1047.jpg')\n",
    "#image = mpimg.imread('./writeup_images/frame0.jpg')\n",
    "#image = mpimg.imread('./test_images/straight_lines2.jpg')\n",
    "\n",
    "\n",
    "undist = cv2.undistort(image, mtx, dist, None, mtx)\n",
    "binary = do_threshold(undist, s_thresh=(130, 255), sx_thresh=(20, 100))\n",
    "warped, Minv = transform(binary)\n",
    "newwarp = findLane(warped, Minv)\n",
    "final = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)\n",
    "\n",
    "\"\"\" \n",
    "pts = np.array([[265,683], [1060,683], [738,480], [552, 480]], np.int32)\n",
    "pts = pts.reshape((-1,1,2))\n",
    "cv2.polylines(undist,[pts],True,(0,255,0), 3)\n",
    "cv2.rectangle(warped, (300,693), (980,350), (0,255,0),3)\n",
    "\"\"\"\n",
    "f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))\n",
    "f.tight_layout()\n",
    "\n",
    "ax1.imshow(undist,cmap='gray')\n",
    "ax1.set_title('Original Image', fontsize=30)\n",
    "\n",
    "ax2.imshow(final,cmap='gray')\n",
    "ax2.set_title('Poly Image', fontsize=30)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "leftLine = Line()\n",
    "rightLine = Line()\n",
    "\n",
    "f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))\n",
    "f.tight_layout()\n",
    "\n",
    "#image = mpimg.imread('./camera_cal/calibration2.jpg')\n",
    "image = mpimg.imread('./test_images/test6.jpg')\n",
    "out = pipeline(image)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#image = cv2.imread('./test_images/test5.jpg')\n",
    "image_name = \"./test_images/straight_lines2\"\n",
    "image = cv2.imread(image_name+'.jpg')\n",
    "\n",
    "undist = cv2.undistort(image, mtx, dist, None, mtx)\n",
    "\n",
    "cv2.imwrite(image_name+\"_undistort3.jpg\", undist)\n",
    "\n",
    "warped, Minv = transform(image)\n",
    "\n",
    "cv2.imwrite(image_name+\"_transform3.jpg\", warped)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def show_s_channel(img, g_thresh=(100, 255), s_thresh=(170, 255), sx_thresh=(20, 100)):\n",
    "    img = np.copy(img)    \n",
    "\n",
    "    # Convert to HLS color space and separate the V channel\n",
    "    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS).astype(np.float)\n",
    "    h_channel = hls[:,:,0]\n",
    "    l_channel = hls[:,:,1]\n",
    "    s_channel = hls[:,:,2]\n",
    "        \n",
    "    # Sobel x\n",
    "    sobelx = cv2.Sobel(l_channel, cv2.CV_64F, 1, 0) # Take the derivative in x\n",
    "    abs_sobelx = np.absolute(sobelx) # Absolute x derivative to accentuate lines away from horizontal\n",
    "    scaled_sobel = np.uint8(255*abs_sobelx/np.max(abs_sobelx))\n",
    "    \n",
    "    # Threshold x gradient\n",
    "    sxbinary = np.zeros_like(scaled_sobel)\n",
    "    sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])] = 1\n",
    "    \n",
    "    # Threshold color channel\n",
    "    s_binary = np.zeros_like(s_channel)\n",
    "    s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1\n",
    "    \n",
    "    # Threshold color channel\n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "    g_binary = np.zeros_like(gray)\n",
    "    g_binary[(gray > g_thresh[0]) & (gray <= g_thresh[1])] = 1\n",
    "    \n",
    "    R = image[:,:,0]\n",
    "    r_binary = np.zeros_like(R)\n",
    "    r_binary[(R > 100) & (R <= 255)] = 1\n",
    "    \n",
    "    thresh = (13, 10)\n",
    "    h_binary = np.zeros_like(h_channel)\n",
    "    h_binary[(h_channel > thresh[0]) & (h_channel <= thresh[1])] = 1\n",
    "    \n",
    "    f, (ax1, ax2, ax3, ax4) = plt.subplots(1, 4, figsize=(20,10))\n",
    "    ax1.set_title('s binary')\n",
    "    ax1.imshow(s_binary)\n",
    "\n",
    "    ax2.set_title('h binary')\n",
    "    ax2.imshow(h_binary, cmap='gray')\n",
    "    \n",
    "    \n",
    "    \n",
    "    ax3.set_title('sx grident')\n",
    "    ax3.imshow(s_binary, cmap='gray')\n",
    "    \n",
    "    combined = np.zeros_like(s_binary)\n",
    "    #combined[((sxbinary == 1) | ((s_binary == 1) & (h_binary == 1)))] = 1\n",
    "    combined[((sxbinary == 1) | ((s_binary == 1) ))] = 1\n",
    "    \n",
    "    ax4.set_title('combine')\n",
    "    ax4.imshow(combined, cmap='gray')\n",
    "    \n",
    "    # Stack each channel\n",
    "    # Note color_binary[:, :, 0] is all 0s, effectively an all black image. It might\n",
    "    # be beneficial to replace this channel with something else.\n",
    "    #color_binary = np.dstack(( np.zeros_like(sxbinary), sxbinary, s_binary))\n",
    "    \n",
    "    #combined = np.zeros_like(s_binary)\n",
    "    #combined[((sxbinary == 1) | (s_binary == 1))] = 1\n",
    "    \n",
    "    \n",
    "    return s_binary"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "\n",
    "image = cv2.imread('./test_images/frame1045.jpg')\n",
    "binary = show_s_channel(image, g_thresh=(80, 255), s_thresh=(170, 255), sx_thresh=(20, 100))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[MoviePy] >>>> Building video challenge_output.mp4\n",
      "[MoviePy] Writing video challenge_output.mp4\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 485/485 [01:24<00:00,  5.33it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[MoviePy] Done.\n",
      "[MoviePy] >>>> Video ready: challenge_output.mp4 \n",
      "\n",
      "CPU times: user 1min 29s, sys: 25 s, total: 1min 54s\n",
      "Wall time: 1min 24s\n"
     ]
    }
   ],
   "source": [
    "leftLine = Line()\n",
    "rightLine = Line()\n",
    "\n",
    "white_output = 'challenge_output.mp4'\n",
    "## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video\n",
    "## To do so add .subclip(start_second,end_second) to the end of the line below\n",
    "## Where start_second and end_second are integer values representing the start and end of the subclip\n",
    "## You may also uncomment the following line for a subclip of the first 5 seconds\n",
    "#clip1 = VideoFileClip(\"test_videos/solidWhiteRight.mp4\").subclip(0,5)\n",
    "clip1 = VideoFileClip(\"./challenge_video.mp4\") #.subclip(37,40)\n",
    "white_clip = clip1.fl_image(pipeline) #NOTE: this function expects color images!!\n",
    "%time white_clip.write_videofile(white_output, audio=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "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.6.1"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}