LaneToSteeringChallenge.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "ename": "AttributeError",
     "evalue": "'NoneType' object has no attribute 'shape'",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mAttributeError\u001b[0m                            Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-1-f168350e20b7>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m      8\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      9\u001b[0m \u001b[0mswheel\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mcv2\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mimread\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'Desktop/steering_wheel_image.jpg'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 10\u001b[1;33m \u001b[0msrows\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mscols\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mch\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mswheel\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mshape\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     11\u001b[0m \u001b[0msmoothed_angle\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;36m0\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     12\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mAttributeError\u001b[0m: 'NoneType' object has no attribute 'shape'"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import time\n",
    "\n",
    "out_examples = 0\n",
    "MOV_AVG_LENGTH = 5\n",
    "\n",
    "\n",
    "swheel = cv2.imread('Desktop/steering_wheel_image.jpg')\n",
    "srows,scols,ch = swheel.shape\n",
    "smoothed_angle = 0\n",
    "\n",
    "def color(inpImage): \n",
    "    # Apply HLS color filtering to filter out white lane lines\n",
    "    hls = cv2.cvtColor(inpImage, cv2.COLOR_BGR2HLS)\n",
    "    lower_white = np.array([0, 160, 10])\n",
    "    upper_white = np.array([255, 255, 255])\n",
    "    mask = cv2.inRange(inpImage, lower_white, upper_white)\n",
    "    hls_result = cv2.bitwise_and(inpImage, inpImage, mask = mask)\n",
    "    \n",
    "    # Convert image to grayscale, apply threshold, blur & extract edges\n",
    "    gray = cv2.cvtColor(hls_result, cv2.COLOR_BGR2GRAY)\n",
    "    ret, thresh = cv2.threshold(gray, 180, 255, cv2.THRESH_BINARY)\n",
    "    blur = cv2.GaussianBlur(thresh,(3, 3), 11)\n",
    "    canny = cv2.Canny(blur, 40, 60)\n",
    "    return canny\n",
    "\n",
    "def sobel_binary(img, sobel_kernel=7, mag_thresh=(3, 255), s_thresh=(170, 255)):\n",
    "    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)\n",
    "    gray = hls[:, :, 1]\n",
    "    s_channel = hls[:, :, 2]\n",
    "    # Binary matrixes creation\n",
    "    sobel_binary = np.zeros(shape=gray.shape, dtype=bool)\n",
    "    s_binary = sobel_binary\n",
    "    combined_binary = s_binary.astype(np.float32)\n",
    "    # Sobel Transform\n",
    "    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)\n",
    "    sobely = 0 \n",
    "    #cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)\n",
    "    sobel_abs = np.abs(sobelx**2 + sobely**2)\n",
    "    sobel_abs = np.uint8(255 * sobel_abs / np.max(sobel_abs))\n",
    "    sobel_binary[(sobel_abs > mag_thresh[0]) & (sobel_abs <= mag_thresh[1])] = 1\n",
    "    # Threshold color channel\n",
    "    s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1\n",
    "    # Combine the two binary thresholds\n",
    "    combined_binary[(s_binary == 1) | (sobel_binary == 1)] = 1\n",
    "    combined_binary = np.uint8(255 * combined_binary / np.max(combined_binary))\n",
    "    return combined_binary\n",
    "\n",
    "def region_of_interest(img):\n",
    "    mask = np.zeros_like(img)\n",
    "    imshape=img.shape\n",
    "    vertices = np.array([[(150,imshape[0]),(590, 440), (680, 440), (imshape[1]-20,imshape[0])]], dtype=np.int32)\n",
    "    cv2.fillPoly(mask, vertices, 255)\n",
    "    masked_image = cv2.bitwise_and(img, mask)\n",
    "    return masked_image\n",
    "\n",
    "def warp(img, src, dst):\n",
    "\n",
    "    src = np.float32([src])\n",
    "    dst = np.float32([dst])\n",
    "    \n",
    "    return cv2.warpPerspective(img, cv2.getPerspectiveTransform(src, dst),\n",
    "                               dsize=img.shape[0:2][::-1], flags=cv2.INTER_LINEAR)\n",
    "\n",
    "\n",
    "def sliding_windown(img_w):\n",
    "\n",
    "    histogram = np.sum(img_w[int(img_w.shape[0] / 2):, :], axis=0)\n",
    "    # Create an output image to draw on and visualize the result\n",
    "    out_img = np.dstack((img_w, img_w, img_w)) * 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(img_w.shape[0] / nwindows)\n",
    "    # Identify the x and y positions of all nonzero pixels in the image\n",
    "    nonzero = img_w.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 = img_w.shape[0] - (window + 1) * window_height\n",
    "        win_y_high = img_w.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) & (\n",
    "            nonzerox < win_xleft_high)).nonzero()[0]\n",
    "        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (\n",
    "            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, img_w.shape[0] - 1, img_w.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",
    "    # 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",
    "    # plt.imshow(out_img)\n",
    "    # plt.plot(left_fitx, ploty, color='yellow')\n",
    "    # plt.plot(right_fitx, ploty, color='yellow')\n",
    "    # plt.xlim(0, 1280)\n",
    "    # plt.ylim(720, 0)\n",
    "\n",
    "    return left_fit, right_fit\n",
    "\n",
    "\n",
    "def fit_from_lines(left_fit, right_fit, img_w):\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 = img_w.nonzero()\n",
    "    nonzeroy = np.array(nonzero[0])\n",
    "    nonzerox = np.array(nonzero[1])\n",
    "    margin = 100\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",
    "    right_lane_inds = (\n",
    "    (nonzerox > (right_fit[0] * (nonzeroy ** 2) + right_fit[1] * nonzeroy + right_fit[2] - margin)) & (\n",
    "    nonzerox < (right_fit[0] * (nonzeroy ** 2) + 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",
    "    return left_fit, right_fit\n",
    "\n",
    "\n",
    "def draw_lines(img, img_w, left_fit, right_fit, perspective):\n",
    "    # Create an image to draw the lines on\n",
    "    warp_zero = np.zeros_like(img_w).astype(np.uint8)\n",
    "    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))\n",
    "    #color_warp_center = np.dstack((warp_zero, warp_zero, warp_zero))\n",
    "\n",
    "    ploty = np.linspace(0, img.shape[0] - 1, img.shape[0])\n",
    "\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",
    "    # 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_center, np.int_([pts]), (0, 255, 0))\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 = warp(color_warp, perspective[1], perspective[0])\n",
    "    # Combine the result with the original image\n",
    "    result = cv2.addWeighted(img, 1, newwarp, 0.2, 0)\n",
    "\n",
    "    color_warp_lines = np.dstack((warp_zero, warp_zero, warp_zero))\n",
    "    cv2.polylines(color_warp_lines, np.int_([pts_right]), isClosed=False, color=(255, 255, 255), thickness=25)\n",
    "    cv2.polylines(color_warp_lines, np.int_([pts_left]), isClosed=False, color=(0, 255, 255), thickness=25)\n",
    "    newwarp_lines = warp(color_warp_lines, perspective[1], perspective[0])\n",
    "\n",
    "    result = cv2.addWeighted(result, 1, newwarp_lines, 1, 0)\n",
    "\n",
    "    # ----- Radius Calculation ------ #\n",
    "\n",
    "    img_height = img.shape[0]\n",
    "    y_eval = img_height\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",
    "    ploty = np.linspace(0, img_height - 1, img_height)\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",
    "\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(\n",
    "        2 * left_fit_cr[0])\n",
    "\n",
    "    right_curverad = (\n",
    "                         (1 + (2 * right_fit_cr[0] * y_eval * ym_per_pix + right_fit_cr[1]) ** 2) ** 1.5) / np.absolute(\n",
    "        2 * right_fit_cr[0])\n",
    "    radius1 = round((float(left_curverad) + float(right_curverad))/2.,2)\n",
    "    \n",
    "    if left_fitx[0] - left_fitx[-1] > 60:\n",
    "        curve_direction = 'Left Curve'\n",
    "        radius=-5729.57795/radius1\n",
    "    elif left_fitx[-1] - left_fitx[0] > 60:\n",
    "        curve_direction = 'Right Curve'\n",
    "        radius=5729.57795/radius1\n",
    "    else:\n",
    "        curve_direction = 'Straight'\n",
    "        radius=5729.57795/radius1\n",
    "\n",
    "    # ----- Off Center Calculation ------ #\n",
    "\n",
    "    lane_width = (right_fit[2] - left_fit[2]) * xm_per_pix\n",
    "    center = (right_fit[2] - left_fit[2]) / 2\n",
    "    off_left = (center - left_fit[2]) * xm_per_pix\n",
    "    off_right = -(right_fit[2] - center) * xm_per_pix\n",
    "    off_center = round((center - img.shape[0] / 2.) * xm_per_pix,2)\n",
    "\n",
    "    # --- Print text on screen ------ #\n",
    "    #if radius < 5000.0:\n",
    "    text = \"Angle = %s [degrees]\\noffcenter = %s [m]\" % (str(radius), str(off_center))\n",
    "    #text = \"radius = -- [m]\\noffcenter = %s [m]\" % (str(off_center))\n",
    "\n",
    "    for i, line in enumerate(text.split('\\n')):\n",
    "        i = 550 + 20 * i\n",
    "        cv2.putText(result, line, (0,i), cv2.FONT_HERSHEY_DUPLEX, 0.5,(255,255,255),1,cv2.LINE_AA)\n",
    "    return result,radius\n",
    "\n",
    "def text(img,angle):\n",
    "    mask = np.zeros_like(img)\n",
    "    cv2.putText(img,str(round(angle)),(100,70), cv2.FONT_HERSHEY_SIMPLEX, 1,(0,0,0),2,cv2.LINE_AA)\n",
    "    res = cv2.addWeighted(img,0.7,mask,0.3,0)\n",
    "    return res\n",
    "\n",
    "\n",
    "cap = cv2.VideoCapture('Desktop/LaneToSteeringChallenge.mp4')\n",
    "\n",
    "# Define the codec and create VideoWriter object\n",
    "fourcc = cv2.VideoWriter_fourcc(*'XVID')\n",
    "out = cv2.VideoWriter('Desktop/LaneToSteeringChallenge.avi',fourcc, 20.0, (1280,720))\n",
    "#out.write(final)\n",
    "while(True):\n",
    "    start=time.time()\n",
    "    ret, frame = cap.read()\n",
    "    if ret is True: \n",
    "        image = cv2.resize(frame,(1280,720),interpolation=cv2.INTER_AREA)\n",
    "    else:\n",
    "        print(\"Getting no frames\")\n",
    "        break\n",
    "    #-------------------------Color & Gradient Threshold------------------------ \n",
    "    edges = color(image)\n",
    "    #edges1 = canny(image)\n",
    "    edges2=sobel_binary(image)\n",
    "    \n",
    "    A= cv2.addWeighted(edges2,0.7,edges,0.3,0)\n",
    "    BW1=cv2.bitwise_and(A, edges2)\n",
    "    img_b=region_of_interest(BW1)\n",
    "\n",
    "    # ---------------------------- Perspective Transform --------------------------\n",
    "\n",
    "    #src = [585, 457], [700, 457], [1110, img_b.shape[0]], [220, img_b.shape[0]]\n",
    "    src = [480, 500], [800, 500], [img_b.shape[1]-50, img_b.shape[0]],  [150, img_b.shape[0]]\n",
    "\n",
    "    line_dst_offset = 200\n",
    "    #src = [595, 500], [685, 500],[1110,img_b.shape[0]],[220, img_b.shape[0]]\n",
    "    dst = [src[3][0] + line_dst_offset, 0], \\\n",
    "          [src[2][0] - line_dst_offset, 0], \\\n",
    "          [src[2][0] - line_dst_offset, src[2][1]], \\\n",
    "          [src[3][0] + line_dst_offset, src[3][1]]\n",
    "    \n",
    "    img_w = warp(img_b, src, dst)\n",
    "    try:\n",
    "        left_fit, right_fit = fit_from_lines(left_fit, right_fit, img_w)\n",
    "        mov_avg_left = np.append(mov_avg_left,np.array([left_fit]), axis=0)\n",
    "        mov_avg_right = np.append(mov_avg_right,np.array([right_fit]), axis=0)\n",
    "        \n",
    "    except Exception:\n",
    "        left_fit, right_fit = sliding_windown(img_w)\n",
    "        mov_avg_left = np.array([left_fit])\n",
    "        mov_avg_right = np.array([right_fit])\n",
    "\n",
    "    left_fit = np.array([np.mean(mov_avg_left[::-1][:,0][0:MOV_AVG_LENGTH]),\n",
    "                        np.mean(mov_avg_left[::-1][:,1][0:MOV_AVG_LENGTH]),\n",
    "                        np.mean(mov_avg_left[::-1][:,2][0:MOV_AVG_LENGTH])])\n",
    "    right_fit = np.array([np.mean(mov_avg_right[::-1][:,0][0:MOV_AVG_LENGTH]),\n",
    "                         np.mean(mov_avg_right[::-1][:,1][0:MOV_AVG_LENGTH]),\n",
    "                         np.mean(mov_avg_right[::-1][:,2][0:MOV_AVG_LENGTH])])\n",
    "    if mov_avg_left.shape[0] > 1000:\n",
    "        mov_avg_left = mov_avg_left[0:MOV_AVG_LENGTH]\n",
    "    if mov_avg_right.shape[0] > 1000:\n",
    "        mov_avg_right = mov_avg_right[0:MOV_AVG_LENGTH]\n",
    "        \n",
    "    final,degrees = draw_lines(frame, img_w, left_fit, right_fit, perspective=[src,dst])    \n",
    "    smoothed_angle += 0.2 * pow(abs((degrees - smoothed_angle)), 2.0 / 3.0) * (degrees - smoothed_angle) / abs(degrees - smoothed_angle)\n",
    "    M = cv2.getRotationMatrix2D((scols/2,srows/2),-smoothed_angle,1)\n",
    "    dst = cv2.warpAffine(swheel,M,(scols,srows)) \n",
    "    steer=text(dst,-smoothed_angle)\n",
    "    final[0:240, 0:240] = steer\n",
    "\n",
    "    #out.write(final)\n",
    "    cv2.imshow('front_view', frame)\n",
    "    cv2.imshow('canny', edges)\n",
    "    cv2.imshow('ROI', img_b)\n",
    "    cv2.imshow('Sky_view', img_w)\n",
    "    cv2.imshow('final', final)\n",
    "    #cv2.imshow(\"steering wheel\", dst)\n",
    "\n",
    "    if cv2.waitKey(1) & 0xFF == ord('q'):\n",
    "        break\n",
    "cap.release()\n",
    "cv2.destroyAllWindows()\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.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}