image_gen-Camera_Center_Cal_Curvature.py.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%run tracker.py.ipynb\n",
    "import numpy as np\n",
    "import cv2\n",
    "import pickle\n",
    "import glob\n",
    "#from tracker import tracker"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# From tracker import tracker"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "dist_pickle = pickle.load(open('calibration_pickle.p','rb'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "mtx = dist_pickle['mtx']\n",
    "dist = dist_pickle['dist']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# play list number 14:27\n",
    "# https://www.youtube.com/watch?v=vWY8YUayf9Q&feature=youtu.be\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def abs_sobel_thresh(img,orient='x',sobel_kernel=3,thresh=(0,255)):\n",
    "    # calculate directional gradient\n",
    "    gray = cv2.cvtColor(img,cv2.COLOR_RGB2GRAY)\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",
    "    \n",
    "    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))\n",
    "    binary_output = np.zeros_like(scaled_sobel)\n",
    "    \n",
    "    binary_output[(scaled_sobel>=thresh[0])&(scaled_sobel<=thresh[1])] = 1\n",
    "    return binary_output\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def mag_thresh(image,sobel_kernel = 3,mag_thresh =(0,255)):\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",
    "    gradmag = np.sqrt(sobelx**2,sobely**2)\n",
    "    scale_factor = np.max(gradmag)/255\n",
    "    gradmag = (gradmag/scale_factor).astype(np.uint8)\n",
    "    binary_output = np.zeros_like(gradmag)\n",
    "    binary_output[(gradmag >= mag_thresh[0]) & (gradmag <= mag_thresh[1])] = 1\n",
    "    return binary_output"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def dir_threshold(image, sobel_kernel = 3, thresh = (0,np.pi/2)):\n",
    "    # calculate gradient direction\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",
    "    with np.errstate(divide='ignore',invalid='ignore'):\n",
    "        absgraddir = np.absolute(np.arctan(sobely/sobelx))\n",
    "        binary_output = np.zeros_like(absgraddir)\n",
    "        #apply threshold\n",
    "        binary_output[(absgraddir >= thresh[0])&(absgraddir <= thresh[1])]=1\n",
    "    return binary_output\n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def color_threshold(image,sthresh = (0,255),vthresh = (0,255)):\n",
    "    hls = cv2.cvtColor(image,cv2.COLOR_RGB2HLS)\n",
    "    s_channel = hls[:,:,2]\n",
    "    s_binary = np.zeros_like(s_channel)\n",
    "    s_binary[(s_channel >= sthresh[0])&(s_channel <= sthresh[1])] = 1\n",
    "    \n",
    "    hsv = cv2.cvtColor(image,cv2.COLOR_RGB2HSV)\n",
    "    v_channel = hsv[:,:,2]\n",
    "    v_binary = np.zeros_like(v_channel)\n",
    "    v_binary[(v_channel >=vthresh[0])&(v_channel <= vthresh[1])] = 1\n",
    "    \n",
    "    output = np.zeros_like(s_channel)\n",
    "    output[(s_binary ==1)&(v_binary ==1)] = 1\n",
    "    return output\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "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)):min(int(center+width),img_ref.shape[1])] = 1\n",
    "    return output"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# make a list of test images\n",
    "images = glob.glob('test_images/test*.jpg')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(1280, 720)\n",
      "(1280, 720)\n",
      "(1280, 720)\n",
      "(1280, 720)\n",
      "(1280, 720)\n",
      "(1280, 720)\n"
     ]
    }
   ],
   "source": [
    "for idx,fname in enumerate(images):\n",
    "    # read in image\n",
    "    img = cv2.imread(fname)\n",
    "    # undistort the image\n",
    "    img = cv2.undistort(img,mtx,dist,None,mtx)\n",
    "    \n",
    "    \n",
    "    preprocessImage = np.zeros_like(img[:,:,0])\n",
    "    gradx = abs_sobel_thresh(img, orient = 'x', thresh =(12,255)) \n",
    "    grady = abs_sobel_thresh(img, orient = 'y', thresh =(25,255))\n",
    "    c_binary = color_threshold(img, sthresh = (100,255), vthresh = (50,255))\n",
    "    preprocessImage[((gradx == 1)&(grady == 1)|(c_binary == 1))] = 255\n",
    "    \n",
    "    img_size =(img.shape[1],img.shape[0])\n",
    "    print(img_size)\n",
    "    bot_width = .76\n",
    "    mid_width = .08\n",
    "    height_pct= .62\n",
    "    bottom_trim= .935\n",
    "    \n",
    " \n",
    "    offset = img_size[0]*.25\n",
    "    src = np.float32([[img.shape[1]*(.5-mid_width/2),img.shape[0]*height_pct],[img.shape[1]*(.5+mid_width/2),img.shape[0]*height_pct],[img.shape[1]*(.5+bot_width/2),img.shape[0]*bottom_trim],[img.shape[1]*(.5-bot_width/2),img.shape[0]*bottom_trim]])\n",
    "    dst = np.float32([[offset, 0], [img_size[0]-offset, 0],[img_size[0]-offset, img_size[1]],[offset, img_size[1]]])\n",
    "    \n",
    "    M = cv2.getPerspectiveTransform(src, dst)\n",
    "    Minv = cv2.getPerspectiveTransform(dst, src)                 \n",
    "    warped = cv2.warpPerspective(preprocessImage, M, img_size, flags=cv2.INTER_LINEAR)\n",
    "    \n",
    "    window_width = 25\n",
    "    window_height = 80\n",
    "    \n",
    "    #set up the overall class to do all the tracking \n",
    "    # Feb 19, resubmission correction after getting feedback from the grader. \n",
    "    # width of solid line in the US is about 3.7 meter, pixel for x direction 1280/2\n",
    "    curve_centers = tracker(Mywindow_width = window_width, Mywindow_height = window_height, Mymargin = 25, My_ym = 30/720, My_xm = 3.7/640,Mysmooth_factor = 15)\n",
    "    \n",
    "    window_centroids = curve_centers.find_window_centroids(warped)\n",
    "    \n",
    "    \n",
    "    # points used to draw all the left and right windows\n",
    "    \n",
    "    l_points = np.zeros_like(warped)\n",
    "    r_points = np.zeros_like(warped)\n",
    "    \n",
    "    # points used to find the left and right lanes\n",
    "    \n",
    "    rightx = []\n",
    "    leftx = []\n",
    "    \n",
    "    \n",
    "    # Go through each level and draw the windows\n",
    "    \n",
    "    for level in range(0,len(window_centroids)):\n",
    "        \n",
    "        leftx.append(window_centroids[level][0])\n",
    "        rightx.append(window_centroids[level][1])\n",
    "        \n",
    "        # window_mask is a function to draw window areas\n",
    "        l_mask = window_mask(window_width, window_height, warped, window_centroids[level][0], level)\n",
    "        r_mask = window_mask(window_width, window_height, warped, window_centroids[level][1], level)\n",
    "        \n",
    "        # add center value found in frame to the list of lane points per left,right\n",
    "        \n",
    "\n",
    "        \n",
    "        # add graphic points from window mask here to total pixels found\n",
    "        \n",
    "        l_points[(l_points == 255)|((l_mask == 1))] = 255\n",
    "        r_points[(r_points == 255)|((r_mask == 1))] = 255\n",
    "        \n",
    "    \n",
    "    #template = np.array(r_points+l_points,np.uint8)\n",
    "    #zero_channel = np.zeros_like(template)\n",
    "    #template = np.array(cv2.merge((zero_channel,template,zero_channel)),np.uint8)\n",
    "    #warpage = np.array(cv2.merge((warped,warped,warped)),np.uint8)\n",
    "    #result = cv2.addWeighted(warpage,1,template,0.5,0.0)\n",
    "                            \n",
    "    # result = warped\n",
    "    \n",
    "    \n",
    "    yvals = range(0,warped.shape[0])\n",
    "    \n",
    "    res_yvals = np.arange(warped.shape[0]-(window_height/2),0,-window_height)\n",
    "    \n",
    "    left_fit = np.polyfit(res_yvals,leftx,2)\n",
    "    left_fitx = left_fit[0]*yvals*yvals + left_fit[1]*yvals + left_fit[2]\n",
    "    left_fitx = np.array(left_fitx,np.int32)\n",
    "    \n",
    "    \n",
    "    right_fit = np.polyfit(res_yvals,rightx,2)\n",
    "    right_fitx = right_fit[0]*yvals*yvals + right_fit[1]*yvals + right_fit[2]\n",
    "    right_fitx = np.array(right_fitx,np.int32)\n",
    "    \n",
    "    left_lane = np.array(list(zip(np.concatenate((left_fitx-window_width/2,left_fitx[::-1]+window_width/2),axis= 0),np.concatenate((yvals,yvals[::-1]),axis = 0))),np.int32)\n",
    "    right_lane = np.array(list(zip(np.concatenate((right_fitx-window_width/2,right_fitx[::-1]+window_width/2),axis= 0),np.concatenate((yvals,yvals[::-1]),axis = 0))),np.int32)\n",
    "    middle_marker = np.array(list(zip(np.concatenate((right_fitx-window_width/2,right_fitx[::-1]+window_width/2),axis = 0),np.concatenate((yvals,yvals[::-1]),axis = 0))),np.int32)\n",
    "    \n",
    "    road = np.zeros_like(img)\n",
    "    road_bkg = np.zeros_like(img)\n",
    "    cv2.fillPoly(road,[left_lane],color = [255,0,0])\n",
    "    cv2.fillPoly(road,[right_lane],color = [0,0,255])\n",
    "    \n",
    "    cv2.fillPoly(road_bkg,[left_lane],color=[255,255,255])\n",
    "    cv2.fillPoly(road_bkg,[right_lane],color = [255,255,255])\n",
    "    \n",
    "    #cv2.fillPoly(road_bkg,[left_lane],color=[255,0,0])\n",
    "    #cv2.fillPoly(road_bkg,[right_lane],color = [0,0,255])\n",
    "    \n",
    "    road_warped = cv2.warpPerspective(road,Minv,img_size,flags = cv2.INTER_LINEAR)\n",
    "    road_warped_bkg = cv2.warpPerspective(road_bkg,Minv,img_size,flags = cv2.INTER_LINEAR)\n",
    "     \n",
    "    base = cv2.addWeighted(img,1.0,road_warped_bkg,-1.0,0.0)\n",
    "    result = cv2.addWeighted(base,1.0,road_warped,1.0,0.0)\n",
    "    \n",
    "    \n",
    "    \n",
    "    ym_per_pix = curve_centers.ym_per_pix\n",
    "    xm_per_pix = curve_centers.xm_per_pix\n",
    "    \n",
    "    curve_fit_cr = np.polyfit(np.array(res_yvals,np.float32)*ym_per_pix,np.array(leftx,np.float32)*xm_per_pix,2)\n",
    "    curverad = ((1+(2*curve_fit_cr[0]*yvals[-1]*ym_per_pix + curve_fit_cr[1])**2)**1.5) /np.absolute(2*curve_fit_cr[0])\n",
    "    \n",
    "    #calculate the offset of the car on the road\n",
    "    \n",
    "    camera_center = (left_fitx[-1] + right_fitx[-1])/2\n",
    "    center_diff = (camera_center-warped.shape[1]/2)*xm_per_pix\n",
    "    side_pos= 'left'\n",
    "    if center_diff <= 0:\n",
    "        side_pos = 'right'\n",
    "\n",
    "        \n",
    "    # draw the text showing curvature, offset, and speed \n",
    "    \n",
    "    cv2.putText(result,'Radius of Curvature = ' +str(round(curverad,3))+'(m)',(50,50),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2)\n",
    "    cv2.putText(result,'Vehicle is '+str(abs(round(center_diff,3)))+'m '+ side_pos+' of center',(50,100) , cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2)\n",
    "    \n",
    "        \n",
    "        \n",
    "                        \n",
    "    write_name = 'test_images/cal_curvature'+str(idx)+'.jpg'\n",
    "    \n",
    "    cv2.imwrite(write_name,result)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "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
}