P2.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[MoviePy] >>>> Building video output_project_video.mp4\n",
      "[MoviePy] Writing video output_project_video.mp4\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|█████████▉| 1260/1261 [02:31<00:00,  8.65it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[MoviePy] Done.\n",
      "[MoviePy] >>>> Video ready: output_project_video.mp4 \n",
      "\n",
      "CPU times: user 8min 28s, sys: 12.4 s, total: 8min 40s\n",
      "Wall time: 2min 31s\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "\n",
       "<video width=\"960\" height=\"540\" controls>\n",
       "  <source src=\"output_project_video.mp4\">\n",
       "</video>\n"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.image as mpimg\n",
    "import glob\n",
    "from copy import copy\n",
    "%matplotlib inline\n",
    "\n",
    "class Lane():\n",
    "    def __init__(self):\n",
    "        # was the line detected in the last frame or not\n",
    "        self.detected = False\n",
    "        #x values for detected line pixels\n",
    "        self.cur_fitx = None\n",
    "        #y values for detected line pixels\n",
    "        self.cur_fity = None\n",
    "        # x values of the last N fits of the line\n",
    "        self.prev_fitx = []\n",
    "        #polynomial coefficients for the most recent fit\n",
    "        self.current_poly = [np.array([False])]\n",
    "        #best polynomial coefficients for the last iteration\n",
    "        self.prev_poly = [np.array([False])]\n",
    "\n",
    "    def average_pre_lanes(self):\n",
    "        tmp = copy(self.prev_fitx)\n",
    "     \n",
    "        tmp.append(self.cur_fitx)\n",
    "        \n",
    "        self.mean_fitx = np.mean(tmp,axis=0)\n",
    "\n",
    "    def append_fitx(self):\n",
    "        if len(self.prev_fitx) == 4:# buffer previous N lines\n",
    "            self.prev_fitx.pop(0)\n",
    "        self.prev_fitx.append(self.mean_fitx)\n",
    "\n",
    "    def process(self, ploty):\n",
    "        self.cur_fity = ploty\n",
    "        self.average_pre_lanes()\n",
    "        self.append_fitx()\n",
    "        self.prev_poly = self.current_poly\n",
    "        \n",
    "left_lane = Lane()\n",
    "right_lane = Lane()\n",
    "\n",
    "def calibration(images):\n",
    "    objpoints = [] \n",
    "    imgpoints = []\n",
    "\n",
    "    objp = np.zeros((9*6,3), np.float32)\n",
    "    objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)\n",
    "    \n",
    "    \n",
    "\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 ret == True:\n",
    "            objpoints.append(objp)\n",
    "            imgpoints.append(corners)\n",
    "\n",
    "\n",
    "    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)\n",
    "    \n",
    "    return mtx, dist\n",
    "\n",
    "def abs_sobel_thresh(img, orient='x', thresh_min=0, thresh_max=255):\n",
    "    # Convert to grayscale\n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)\n",
    "    # Apply x or y gradient with the OpenCV Sobel() function\n",
    "    # and take the absolute value\n",
    "    if orient == 'x':\n",
    "        abs_sobel = np.absolute(cv2.Sobel(gray, cv2.CV_64F, 1, 0))\n",
    "    if orient == 'y':\n",
    "        abs_sobel = np.absolute(cv2.Sobel(gray, cv2.CV_64F, 0, 1))\n",
    "    # Rescale back to 8 bit integer\n",
    "    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))\n",
    "    # Create a copy and apply the threshold\n",
    "    binary_output = np.zeros_like(scaled_sobel)\n",
    "    binary_output[(scaled_sobel >= thresh_min) & (scaled_sobel <= thresh_max)] = 1\n",
    "    \n",
    "    return binary_output\n",
    "\n",
    "def mag_thresh(img, sobel_kernel=3, mag_thresh=(0, 255)):\n",
    "    \n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)\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",
    "\n",
    "    return binary_output\n",
    "\n",
    "def dir_threshold(img, sobel_kernel=3, thresh=(0, np.pi/2)):\n",
    "    \n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)\n",
    "    \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",
    "    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",
    "def hls_select(img, thresh=(0, 255)):\n",
    "    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)\n",
    "    s_channel = hls[:,:,2]\n",
    "    binary_output = np.zeros_like(s_channel)\n",
    "    binary_output[(s_channel > thresh[0]) & (s_channel <= thresh[1])] = 1\n",
    "    return binary_output\n",
    "\n",
    "def warp(img):\n",
    "    img_size = (img.shape[1], img.shape[0])\n",
    "\n",
    "    src = np.float32(\n",
    "        [[194, 719],\n",
    "         [1117, 719],\n",
    "         [705, 461],\n",
    "         [575, 461]])\n",
    "        \n",
    "    dst = np.float32(\n",
    "        [[290, 719],\n",
    "         [990, 719],\n",
    "         [990, 0],\n",
    "         [290, 0]])\n",
    "    \n",
    "    M = cv2.getPerspectiveTransform(src, dst)\n",
    "    Minv = cv2.getPerspectiveTransform(dst,src)\n",
    "    warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)\n",
    "    return warped, Minv\n",
    "\n",
    "\n",
    "def draw_lanes(image, bird_view, left_fitx, right_fitx, ploty, Minv):\n",
    "    # Create an image to draw the lines on\n",
    "    warp_zero = np.zeros_like(bird_view).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",
    "    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",
    "        \n",
    "    # Combine the result with the original image\n",
    "    result = cv2.addWeighted(image, 1, newwarp, 1, 0)\n",
    "    \n",
    "    return result\n",
    "\n",
    "def combined(image):\n",
    "    grad_x = abs_sobel_thresh(image, orient='x', thresh_min=20, thresh_max=100)\n",
    "           \n",
    "    mag_binary = mag_thresh(image, sobel_kernel=5, mag_thresh=(35, 100))\n",
    "           \n",
    "    thresh=(90, 255)\n",
    "    hls = cv2.cvtColor(image, cv2.COLOR_RGB2HLS)\n",
    "    s_channel = hls[:,:,2]\n",
    "    binary_output = np.zeros_like(s_channel)\n",
    "    \n",
    "    binary_output[(s_channel > thresh[0]) & (s_channel <= thresh[1])] = 1\n",
    "        \n",
    "    combined = np.zeros_like(s_channel)\n",
    "    \n",
    "    combined[(grad_x == 1) & (mag_binary == 1)] = 1\n",
    "        \n",
    "    gradient_color_combined = np.zeros_like(s_channel)\n",
    "    \n",
    "    gradient_color_combined[(combined == 1) | (binary_output == 1)] = 1\n",
    "    \n",
    "    c_bi = np.zeros_like(s_channel)\n",
    "    \n",
    "    c_bi[(grad_x == 1) & (binary_output == 1) ] = 2\n",
    "    \n",
    "    ave_binary = (gradient_color_combined + c_bi)\n",
    "    return gradient_color_combined\n",
    "\n",
    "def cal_lane(histogram,bird_view): \n",
    "        \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",
    "    nwindows = 9\n",
    "    window_height = np.int(bird_view.shape[0]//nwindows)\n",
    "    # Identify the x and y positions of all nonzero pixels in the image\n",
    "    nonzero = bird_view.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 = bird_view.shape[0] - (window+1)*window_height\n",
    "        win_y_high = bird_view.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",
    "        \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",
    "    # Generate x and y values for plotting\n",
    "    ploty = np.linspace(0, bird_view.shape[0]-1, bird_view.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",
    "    return left_fitx, right_fitx, ploty, left_fit, right_fit, leftx, rightx\n",
    "        \n",
    "          \n",
    "def cal_curvature(image, ploty, leftx, rightx):\n",
    "   \n",
    "    leftx = leftx[::-1]  # Reverse to match top-to-bottom in y\n",
    "    rightx = rightx[::-1]  # Reverse to match top-to-bottom in y\n",
    "     # choose the maximum y-value, corresponding to the bottom of the image\n",
    "    y_eval = np.max(ploty)    \n",
    "    # Define conversions in x and y from pixels space to meters\n",
    "    \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, leftx*xm_per_pix, 2)\n",
    "    right_fit_cr = np.polyfit(ploty*ym_per_pix, rightx*xm_per_pix, 2)\n",
    "    \n",
    "   \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",
    "    middle_of_the_lanes =  (leftx[-1] + rightx[-1])/2.0\n",
    "    vehicle_position = image.shape[1]/2.0\n",
    "    \n",
    "    distance = np.absolute((vehicle_position - middle_of_the_lanes)*xm_per_pix)\n",
    "    \n",
    "    if (vehicle_position - middle_of_the_lanes) < 0:\n",
    "        cv2.putText(image,'Left radius of curvature  = %.2f m'%(left_curverad),(50,50), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "        cv2.putText(image,'Right radius of curvature  = %.2f m'%(right_curverad),(50,100), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "        cv2.putText(image,'Vehicle is %.2fm left of center'%(distance),(50,150), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "        \n",
    "    elif (vehicle_position - middle_of_the_lanes) > 0:\n",
    "        cv2.putText(image,'Left radius of curvature  = %.2f m'%(left_curverad),(50,50), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "        cv2.putText(image,'Right radius of curvature  = %.2f m'%(right_curverad),(50,100), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "        cv2.putText(image,'Vehicle is %.2fm right of center'%(distance),(50,150), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "   \n",
    "    else:\n",
    "        cv2.putText(image,'Left radius of curvature  = %.2f m'%(left_curverad),(50,50), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "        cv2.putText(image,'Right radius of curvature  = %.2f m'%(right_curverad),(50,100), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "        cv2.putText(image,'Vehicle is Straight on the road',(50,150), cv2.FONT_HERSHEY_SIMPLEX, 1,(255,255,255),2,cv2.LINE_AA)\n",
    "\n",
    "def detectorORtracker(state,left_fitx, right_fitx, left_fit, right_fit):\n",
    "   #tracker\n",
    "    if state==1:\n",
    "        std_value = np.std(right_fitx - left_fitx)\n",
    "        if std_value < 85:\n",
    "            left_lane.detected = True\n",
    "            right_lane.detected = True\n",
    "            left_lane.current_poly = left_fit\n",
    "            right_lane.current_poly = right_fit\n",
    "            left_lane.cur_fitx = left_fitx\n",
    "            right_lane.cur_fitx = right_fitx\n",
    "            # global tt\n",
    "            # tt = tt + 1\n",
    "        else:\n",
    "            left_lane.detected = False\n",
    "            right_lane.detected = False\n",
    "            left_lane.current_poly = left_lane.prev_poly\n",
    "            right_lane.current_poly = right_lane.prev_poly\n",
    "            left_lane.cur_fitx = left_lane.prev_fitx[-1]\n",
    "            right_lane.cur_fitx = right_lane.prev_fitx[-1]\n",
    "    #deterctor\n",
    "    else:\n",
    "        std_value = np.std(right_fitx - left_fitx)\n",
    "        if std_value < 85:\n",
    "            left_lane.current_poly = left_fit\n",
    "            right_lane.current_poly = right_fit\n",
    "            left_lane.cur_fitx = left_fitx\n",
    "            right_lane.cur_fitx = right_fitx\n",
    "            left_lane.detected = True\n",
    "            right_lane.detected = True\n",
    "        else:\n",
    "            left_lane.current_poly = left_lane.prev_poly\n",
    "            right_lane.current_poly = right_lane.prev_poly\n",
    "            if len(left_lane.prev_fitx) > 0:\n",
    "                left_lane.cur_fitx = left_lane.prev_fitx[-1]\n",
    "                right_lane.cur_fitx = right_lane.prev_fitx[-1]\n",
    "            else:\n",
    "                left_lane.cur_fitx = left_fitx\n",
    "                right_lane.cur_fitx = right_fitx\n",
    "            left_lane.detected = False\n",
    "            right_lane.detected = False   \n",
    "    \n",
    "def operation(image):     \n",
    "\n",
    "    undistort_image = cv2.undistort(image, mtx, dist, None, mtx)\n",
    "\n",
    "    ave_binary = combined(undistort_image)\n",
    "    \n",
    "    bird_view, Minv = warp(ave_binary)\n",
    "    \n",
    "    histogram = np.sum(bird_view[bird_view.shape[0]//2:,:], axis=0)\n",
    "    \n",
    "    left_fitx, right_fitx, ploty, left_fit, right_fit, leftx, rightx = cal_lane(histogram, bird_view)\n",
    "    \n",
    "    if left_lane.detected:  # start tracker\n",
    "        detectorORtracker(1,left_fitx, right_fitx, left_fit, right_fit)\n",
    "    else:                   # start detector\n",
    "        detectorORtracker(2,left_fitx, right_fitx, left_fit, right_fit)\n",
    "    \n",
    "    left_lane.process(ploty)\n",
    "    right_lane.process(ploty)\n",
    "    \n",
    "    cal_curvature(image, ploty, left_lane.mean_fitx, right_lane.mean_fitx)\n",
    "    \n",
    "    detected_image = draw_lanes(image, bird_view, left_lane.mean_fitx, right_lane.mean_fitx, ploty, Minv)\n",
    "       \n",
    "    return detected_image\n",
    "\n",
    "img_for_calibration = glob.glob('/home/nsslab/SDCND/Project2/images_for_calibration/calibration*.jpg')\n",
    "mtx, dist = calibration(img_for_calibration)\n",
    "\n",
    "\n",
    "image = mpimg.imread('/home/nsslab/SDCND/Project2/test_images/test2.jpg')   \n",
    "    \n",
    "from moviepy.editor import VideoFileClip\n",
    "from IPython.display import HTML\n",
    "\n",
    "white_output = 'output_project_video.mp4'\n",
    "clip1 = VideoFileClip(\"/home/nsslab/SDCND/Project2/project_video.mp4\")\n",
    "white_clip = clip1.fl_image(operation) \n",
    "%time white_clip.write_videofile(white_output , audio=False)\n",
    "\n",
    "HTML(\"\"\"\n",
    "<video width=\"960\" height=\"540\" controls>\n",
    "  <source src=\"{0}\">\n",
    "</video>\n",
    "\"\"\".format(white_output))\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}