498_LidarRead_traditionalmethod.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import struct\n",
    "import datetime as dt\n",
    "import glob\n",
    "import os\n",
    "from collections import namedtuple\n",
    "import matplotlib.image as mpimg\n",
    "import cv2\n",
    "\n",
    "import numpy as np\n",
    "# import pykitti.utils as utils"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# fileName = '/Users/GF/Downloads/data_road_velodyne/testing/velodyne/um_000000.bin'\n",
    "# with open(fileName, mode='rb') as file: # b is important -> binary\n",
    "#     fileContent = file.read()\n",
    "# struct.unpack(\"iiiii\", fileContent[:20])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "...\n"
     ]
    }
   ],
   "source": [
    "def get_velo_scans(velo_file):\n",
    "    \"\"\"read velodyne [x,y,z,reflectance] scan data from binary files.\"\"\"\n",
    "    \"\"\"parse velodyne binary files into arrays.\"\"\"\n",
    "    scan = np.fromfile(velo_file, dtype='float32')\n",
    "    return scan.reshape((-1, 4))\n",
    "        \n",
    "# def velo(data_path):\n",
    "#         \"\"\"Generator to read velodyne [x,y,z,reflectance] scan data from binary files.\"\"\"\n",
    "#         # Find all the Velodyne files\n",
    "#         velo_path = os.path.join(\n",
    "#             data_path, 'velodyne_points', 'data', '*.bin')\n",
    "#         velo_files = sorted(glob.glob(velo_path))\n",
    "\n",
    "#         # Subselect the chosen range of frames, if any\n",
    "# #         if self.frames is not None:\n",
    "# #             velo_files = [velo_files[i] for i in self.frames]\n",
    "\n",
    "#         # Return a vgenerator yielding Velodyne scans.\n",
    "#         # Each scan is a Nx4 array of [x,y,z,reflectance]\n",
    "#         return get_velo_scans(velo_files)\n",
    "def sobel(img, thresh_min=0, thresh_max=255): # use the threshold values you found to replace 0 and 255\n",
    "    #1. convert the image to gray scale\n",
    "    #2. Gaussian blur the image\n",
    "    #3. Use Sobel to find derievatives for both X and Y Axis\n",
    "    #4. Use addweight to combine the results\n",
    "    #5. Convert each pixel to unint8, then apply threshold to get binary image\n",
    "    ## TODO\n",
    "    img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "    img_blur = cv2.GaussianBlur(img_gray, (5, 5), 0)\n",
    "    #name your result to be sobelX and sobelY\n",
    "    sobelX = cv2.Sobel(img_blur,cv2.CV_8U,1,0,3)\n",
    "    sobelY = cv2.Sobel(img_blur,cv2.CV_8U,0,1,3)\n",
    "    \n",
    "    ####\n",
    "    \n",
    "    combined = cv2.addWeighted(sobelX, 0.9, sobelY, 0.1, 0)\n",
    "    #scaled_lap = np.uint8(255*combined/np.max(combined)) #for some reason, scaled_lap was only containing 0s and 2s\n",
    "    binary_output = np.zeros_like(combined)\n",
    "    binary_output[(combined >= thresh_min) & (combined <= thresh_max)] = 1\n",
    "    #for i in range(0,len(scaled_lap)):\n",
    "    #    for j in range(0,len(scaled_lap[i])):\n",
    "    #        print(scaled_lap[i][j])\n",
    "\n",
    "    return binary_output\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "def findConvCenter(image):\n",
    "    \n",
    "    # window settings, can be changed if you want. \n",
    "    window_width = 30 \n",
    "    window_height = rawImg.shape[0] / 10.0 # Break image into 10 vertical layers \n",
    "    margin = 80 # How much to shift left or right from last found line segment for searching\n",
    "    window = np.ones(window_width) # Create our window template that we will use for convolutions\n",
    "    convThres = 100 # filtered out back ground noise\n",
    "\n",
    "    #Parameters for finding the start points \n",
    "    vertical_ratio = 1.0/2  \n",
    "    horizontal_ratio = 0.15\n",
    "    \n",
    "    \n",
    "    image = np.uint8(255*image/np.max(image))\n",
    "\n",
    "    #windowCentroids = [] #Output array that store the (left,right) window centroid positions per level\n",
    "    leftCentroids = []\n",
    "    rightCentroids = []\n",
    "\n",
    "    # First find the two starting positions for the left and right lane by using np.sum to get the vertical image slice\n",
    "    # and then np.convolve the vertical image slice with the window template \n",
    "    ## TODO\n",
    "    image_bottom = image[int(image.shape[0]/2):(image.shape[0]-1),:]\n",
    "    sum_arr = np.sum(image_bottom,axis=0,dtype=np.int32)\n",
    "    conv_arr = np.convolve(sum_arr,window,'same')\n",
    "    \n",
    "    leftCenterX = -1\n",
    "    rightCenterX = -1\n",
    "    \n",
    "    center_coord = image.shape[1]/2\n",
    "    while(leftCenterX == -1 or rightCenterX == -1):\n",
    "        max_val = np.argmax(conv_arr)\n",
    "        if(max_val < center_coord+450 and max_val > center_coord-450):\n",
    "            if(max_val < center_coord and leftCenterX == -1):\n",
    "                leftCenterX = max_val\n",
    "            elif(max_val >= center_coord and rightCenterX == -1):\n",
    "                rightCenterX = max_val\n",
    "        conv_arr[max_val] = 0\n",
    "    #450 pixels to either side of center\n",
    "    \n",
    "    leftCenterY = image.shape[0] - (window_height/2)\n",
    "    rightCenterY = leftCenterY\n",
    "    ####\n",
    "    # Add what we found for the first layer\n",
    "    leftCentroids.append((leftCenterX, leftCenterY))\n",
    "    rightCentroids.append((rightCenterX, rightCenterY))\n",
    "\n",
    "    #Count how many empty layers for left and right line\n",
    "    leftEmptyCount, rightEmptyCount = 0, 0\n",
    "    \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",
    "        ## TODO\n",
    "        image_window = image[int(image.shape[0]-(level+1)*window_height):int(image.shape[0]-level*window_height-1),:]\n",
    "        sum_arr = np.sum(image_window,axis=0,dtype=np.int32)\n",
    "        conv_arr = np.convolve(sum_arr,window,'same')\n",
    "        ####\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",
    "        ## TODO\n",
    "        left_coord = leftCenterX-margin\n",
    "        if(left_coord < 0):\n",
    "            left_coord = 0\n",
    "        right_coord = leftCenterX+margin\n",
    "        if(right_coord >= image.shape[1]):\n",
    "            right_coord = image.shape[1] - 1\n",
    "        conv_arr_left = conv_arr[left_coord:right_coord]\n",
    "        newleftCenterX = np.argmax(conv_arr_left)\n",
    "        if(conv_arr_left[newleftCenterX] < convThres):\n",
    "            leftCenterX = leftCenterX\n",
    "        else:\n",
    "            leftCenterX = newleftCenterX + left_coord\n",
    "        #print(left_coord,right_coord, leftCenterX, conv_arr_left)\n",
    "        ####   \n",
    "        # Find the best right centroid by using past right center as a reference\n",
    "        #if last layer not found, extend margin and reuse old refrence\n",
    "        ## TODO\n",
    "        left_coord = rightCenterX-margin\n",
    "        if(left_coord < 0):\n",
    "            left_coord = 0\n",
    "        right_coord = rightCenterX+margin\n",
    "        if(right_coord >= image.shape[1]):\n",
    "            right_coord = image.shape[1] - 1\n",
    "        conv_arr_right = conv_arr[left_coord:right_coord]\n",
    "        newrightCenterX = np.argmax(conv_arr_right)\n",
    "        if(conv_arr_right[newrightCenterX] < convThres):\n",
    "            rightCenterX = rightCenterX\n",
    "        else:\n",
    "            rightCenterX = newrightCenterX + left_coord\n",
    "        ####\n",
    "        # Add what we found for that layer\n",
    "        ## TODO\n",
    "        #print(leftCenterX, leftCenterY, rightCenterX, rightCenterY)\n",
    "        leftCenterY = leftCenterY - window_height\n",
    "        rightCenterY = leftCenterY\n",
    "        leftCentroids.append((leftCenterX, leftCenterY))\n",
    "        rightCentroids.append((rightCenterX, rightCenterY))\n",
    "        ####\n",
    "    ####\n",
    "    #return two list of tuples \n",
    "    return leftCentroids, rightCentroids\n",
    "\n",
    "def window_mask(width, height, img_ref, centerX, centerY):\n",
    "    output = np.zeros_like(img_ref)\n",
    "    #output[int(img_ref.shape[0]-centerY-height/2):int(img_ref.shape[0]-centerY+height/2),max(0,int(centerX-width/2)):min(int(centerX+width/2),img_ref.shape[1])] = 1\n",
    "    output[int(centerY-height/2):int(centerY+height/2),max(0,int(centerX-width/2)):min(int(centerX+width/2),img_ref.shape[1])] = 1\n",
    "    return output\n",
    "\n",
    "\n",
    "def plotWindows(warped):\n",
    "    leftCentroids, rightCentroids= findConvCenter(warped)\n",
    "    \n",
    "    # Points used to draw all the left and right windows\n",
    "    r_points = np.zeros_like(warped)\n",
    "    l_points = np.zeros_like(warped)         \n",
    "    \n",
    "    if len(leftCentroids) > 0 and len(rightCentroids) > 0:\n",
    "        if len(leftCentroids) > 0:\n",
    "            # Go through each level and draw the windows \n",
    "            for level in range(0,len(leftCentroids)):\n",
    "                # Window_mask is a function to draw window areas\n",
    "                l_mask = window_mask(window_width,window_height,warped,leftCentroids[level][0],leftCentroids[level][1])\n",
    "                # Add graphic points from window mask here to total pixels found \n",
    "                l_points[(l_points == 255) | ((l_mask == 1) ) ] = 255\n",
    "\n",
    "        if len(rightCentroids) > 0:\n",
    "            # Go through each level and draw the windows \n",
    "            for level in range(0,len(rightCentroids)):\n",
    "                # Window_mask is a function to draw window areas\n",
    "                r_mask = window_mask(window_width,window_height,warped,rightCentroids[level][0],rightCentroids[level][1])\n",
    "                # Add graphic points from window mask here to total pixels found \n",
    "                r_points[(r_points == 255) | ((r_mask == 1) ) ] = 255\n",
    "\n",
    "        # Draw the results\n",
    "        template = np.array(r_points+l_points,np.uint8) # add both left and right window pixels together\n",
    "        zero_channel = np.zeros_like(template) # create a zero color channel\n",
    "        template = np.array(cv2.merge((zero_channel,template,zero_channel)),np.uint8) # make window pixels green\n",
    "        warpage= np.dstack((warped, warped, warped))*255 # making the original road pixels 3 color channels\n",
    "        output = cv2.addWeighted(warpage, 1, template, 0.5, 0.0) # overlay the orignal road image with window results\n",
    "    #If no window centers found, just display orginal road image\n",
    "    else:\n",
    "        output = np.array(cv2.merge((warped,warped,warped)),np.uint8)\n",
    "    return output\n",
    "\n",
    "\n",
    "def drawPoly(originImg, warpImg, leftCentroids, rightCentroids, Minv):\n",
    "    #Generate polynomial functions\n",
    "    ##TO DO\n",
    "    h,w = warpImg.shape[:2]\n",
    "    leftX = np.array([i[0] for i in leftCentroids])\n",
    "    leftY = np.array([i[1] for i in leftCentroids])\n",
    "    left_eq = np.polyfit(leftX,leftY,2)\n",
    "    rightX = np.array([i[0] for i in rightCentroids])\n",
    "    rightY = np.array([i[1] for i in rightCentroids])\n",
    "    right_eq = np.polyfit(rightX,rightY,2)\n",
    "    ####\n",
    "    \n",
    "    # Generate y values for plotting\n",
    "    plotY = np.linspace(0, warpImg.shape[0]-1, warpImg.shape[0])\n",
    "    # Generate X values for polynomial line \n",
    "    ## TO DO\n",
    "    leftPlotX = ((left_eq[1]*-1) + np.sqrt(np.abs(left_eq[1]**2 - 4*left_eq[0]*(left_eq[2]-plotY)))) / (2 * left_eq[0])\n",
    "    rightPlotX = ((right_eq[1]*-1) + np.sqrt(np.abs(right_eq[1]**2 - 4*right_eq[0]*(right_eq[2]-plotY)))) / (2 * right_eq[0])\n",
    "    \n",
    "    ####\n",
    "    \n",
    "    #filter out the out of boundary points\n",
    "    ##TO DO\n",
    "    width = warpImg.shape[1]\n",
    "    leftPlotX[leftPlotX<0] = 0\n",
    "    leftPlotX[leftPlotX>=width] = width-1\n",
    "    rightPlotX[rightPlotX<0] = 0\n",
    "    rightPlotX[rightPlotX>=width] = width-1\n",
    "    ####\n",
    "    \n",
    "    #Generate (X,Y) points for plotting\n",
    "    leftPts = np.array([np.transpose(np.vstack([leftPlotX, plotY]))])\n",
    "    rightPts = np.array([np.flipud(np.transpose(np.vstack([rightPlotX, plotY])))])\n",
    "    points = np.hstack((leftPts, rightPts))\n",
    "    #plot to warped image\n",
    "    ##To DO\n",
    "    #warpedpoints = cv2.warpPerspective(points, Minv, (w,h), flags=cv2.INTER_LINEAR)\n",
    "    plottedImg = cv2.fillPoly(warpImg,np.int32([points]),255)\n",
    "    warpedImg = cv2.warpPerspective(plottedImg, Minv, (w,h), flags=cv2.INTER_LINEAR)\n",
    "    newImg = cv2.addWeighted(originImg, 1, warpedImg, .5, 0)\n",
    "    ####\n",
    "    \n",
    "    #return the new image and the coefficients of two polynomial equations \n",
    "    return newImg, leftPlotX, rightPlotX\n",
    "\n",
    "\n",
    "print ('...')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 64,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 64,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#filename = '/Users/GF/Downloads/data_road_velodyne-2/training/velodyne/um_000003.bin'\n",
    "#get lidar data from file and put it into a numpy array, also get camera image and nn output\n",
    "filename = '/Users/Mike/Desktop/data_road_velodyne/training/velodyne/um_000022.bin'\n",
    "camera_filename = '/Users/Mike/Desktop/data_road_gray/training/image_0/um_000022.png'\n",
    "nn_filename = '/Users/Mike/OneDrive/School/Illinois/SVN/ece498sm/project/nn_22.png'\n",
    "cam_img = np.fromfile(camera_filename, dtype='float32') #cv2.imread(camera_filename)\n",
    "nn_img = np.fromfile(nn_filename, dtype='float32') #cv2.imread(camera_filename)\n",
    "arr = get_velo_scans(filename)\n",
    "new_arr = np.full((375,1242,4),(0,0,0,255))\n",
    "#for each data point, round it to the nearest pixel value and insert it into the img\n",
    "for point in arr:\n",
    "    if(point[3] > 0 and point[0] > 0 and point[0] <= 37.4 and abs(point[1]) < 9.95 and point[2] < 0):\n",
    "        x_coord = int(round(point[1]*20)+200)\n",
    "        y_coord = int(round(point[0]*10))\n",
    "        new_arr[374-y_coord][820-x_coord] = (0,int(point[3]*255),0,255)\n",
    "cv2.imwrite(\"out22.png\", new_arr)\n",
    "img = np.float32(new_arr)\n",
    "#filter the image for edge detection\n",
    "thresh_min1 = 0\n",
    "thresh_max1 = 255\n",
    "bw_img = sobel(img, thresh_min1, thresh_max1)\n",
    "#detect lanes from edge detected image\n",
    "leftCentroids, rightCentroids = findConvCenter(bw_img)\n",
    "\n",
    "#cv2.imwrite(\"out3_bw.png\", bw_img)\n",
    "\n",
    "#warp perspctive points, manually found from picture\n",
    "x3, y3 = 655, 299\n",
    "x1, y1 = 657, 243\n",
    "x4, y4 = 518, 300\n",
    "x2, y2 = 516, 248\n",
    "\n",
    "#400,400\n",
    "#1242,375 .9375\n",
    "\n",
    "dst = np.float32([(708,260), #109 #279\n",
    "                  (343,263), #150 101\n",
    "                  (788,332), #284\n",
    "                  (165,325)])   \n",
    "src = np.float32([(x1,y1),(x2,y2),(x3,y3),(x4,y4)])\n",
    "#create perspective transforms\n",
    "M = cv2.getPerspectiveTransform(src,dst)\n",
    "Minv = cv2.getPerspectiveTransform(dst,src)\n",
    "#warp image\n",
    "warpedImg = cv2.warpPerspective(img, M, (1242,375), flags=cv2.INTER_LINEAR)\n",
    "cv2.imwrite(\"out22_w.png\", warpedImg)\n",
    "#newImg = cv2.addWeighted(cam_img, 1, warpedImg, .5, 0)\n",
    "#draw detected lane and combine it with camera image or with output of neural network\n",
    "#imgOut, leftX, rightX = drawPoly(cam_img, img, leftCentroids, rightCentroids, M)\n",
    "imgOut, leftX, rightX = drawPoly(nn_img, img, leftCentroids, rightCentroids, M)\n",
    "cv2.imwrite(\"out22_comb.png\", imgOut)\n",
    "#cv2.imwrite(\"out3_comb.png\", camImg)\n",
    "        \n",
    "# scan = np.fromfile(filename, dtype='uint8')\n",
    "# cv2.imwrite('scan.png', scan[:640000].reshape(64,10000))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "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.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}