Advanced_Lane_Lines.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Import common libs\n",
    "\n",
    "Most of the code in this notebook needs these imports"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.image as mpimg\n",
    "\n",
    "%matplotlib inline\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Transform class\n",
    "\n",
    "Wrapper around the the camera distortion maps and perspective transform matrix, along with the transformation methods."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import glob\n",
    "\n",
    "class Transform():\n",
    "    ''' Transform is used to both undistort images \n",
    "       (from camera distortion) and to to do\n",
    "       perspective transforms.\n",
    "    '''\n",
    "    def __init__(self):\n",
    "        \n",
    "        # undistortion\n",
    "        self.undistortMapX = None\n",
    "        self.undistortMapY = None\n",
    "        \n",
    "        # points used to generate perspective transform\n",
    "        self.perspectiveSrcPoints = []\n",
    "        self.perspectiveDestPoints = []\n",
    "        \n",
    "        # perspective transform matris (and inverse)\n",
    "        self.perspectiveMatrix = None\n",
    "        self.perspectiveMatrixInv = None\n",
    "    \n",
    "        # chessboard image file names used for calibration and corners found.\n",
    "        self.chessboard_imgs_and_corners = []\n",
    "        \n",
    "        # termination criteria for \"cornerSubPix\": max iteratons = 30, epsilon = 0.001\n",
    "        self._corner_sub_pix_criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "        \n",
    "        \n",
    "    def calibrate_with_chessboards(self, glob_pattern, nx, ny, save_corners=False):\n",
    "        ''' Camera calibration. \n",
    "        \n",
    "        Args:\n",
    "            glob_pattern: Patern passed to \"glob\" to find chessboard images to use for calibration.\n",
    "            nx: Number of horizontal intescetions in the chessboard images.\n",
    "            ny: Number of veritical intersections in the chessboard images.\n",
    "            save_corners: Save the filename and found cornders in the \"chessboard_imgs_and_corners\" property. Default is False.\n",
    "        '''\n",
    "        # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)\n",
    "        objp = np.zeros((nx*ny,3), np.float32)\n",
    "        objp[:,:2] = np.mgrid[0:nx,0:ny].T.reshape(-1,2)\n",
    "        \n",
    "        # Arrays to store object points and image points from all the images.\n",
    "        objpoints = [] # 3d point in real world space\n",
    "        imgpoints = [] # 2d points in image plane.\n",
    "        \n",
    "        # Get image file paths matching \"glob_pattern\"\n",
    "        images = glob.glob(glob_pattern)\n",
    "        \n",
    "        # save shape of first image found\n",
    "        img_shape = None\n",
    "        \n",
    "        # loop over images and find chessboard corners\n",
    "        for fname in images:\n",
    "            img = cv2.imread(fname)\n",
    "            gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)\n",
    "    \n",
    "            # Find the chess board corners\n",
    "            ret, corners = cv2.findChessboardCorners(gray, (nx,ny), None)\n",
    "            \n",
    "            # If found, add object points, image points (after refining them)\n",
    "            if ret == True:\n",
    "                objpoints.append(objp)\n",
    "                \n",
    "                corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),self._corner_sub_pix_criteria)\n",
    "                imgpoints.append(corners2)\n",
    "                \n",
    "                if img_shape is None:\n",
    "                    img_shape = (img.shape[1], img.shape[0])\n",
    "                \n",
    "                if save_corners:\n",
    "                    self.chessboard_imgs_and_corners.append((fname, corners2))\n",
    "        \n",
    "        # calibrate camera using data found processing chessboard images\n",
    "        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_shape, None, None)\n",
    "        \n",
    "        # Get the undistort maps to return.\n",
    "        self.undistortMapX, self.undistortMapY = cv2.initUndistortRectifyMap(mtx,dist,None,None,img_shape,5)\n",
    "    \n",
    "    def init_perspective_transform(self, src_points, dest_points):\n",
    "        ''' Initialize perspective transformation.\n",
    "        \n",
    "        Args:\n",
    "            src_points:  Points in perspective image that represents a square.\n",
    "            dest_points: Points in desintaion image that represents that same square.\n",
    "        '''\n",
    "        self.perspectiveSrcPoints = src_points\n",
    "        self.perspectiveDestPoints = dest_points\n",
    "        self.perspectiveMatrix = cv2.getPerspectiveTransform(self.perspectiveSrcPoints, self.perspectiveDestPoints)\n",
    "        self.perspectiveMatrixInv = np.linalg.inv(self.perspectiveMatrix)\n",
    "    \n",
    "    def remove_distortion(self, image):\n",
    "        ''' Remove camera distortion from the image.\n",
    "        '''\n",
    "        if self.undistortMapX is not None:\n",
    "            return cv2.remap(image, self.undistortMapX, self.undistortMapY, cv2.INTER_LINEAR)\n",
    "        else:\n",
    "            return image\n",
    "    \n",
    "    def warp_perspective(self, image):\n",
    "        ''' Warp image to from \"perspective\" to \"overhead\" image.\n",
    "        '''\n",
    "        return cv2.warpPerspective(image, self.perspectiveMatrix, (image.shape[1], image.shape[0]), flags=cv2.INTER_LINEAR)\n",
    "    \n",
    "    def unwarp_perspective(self, image):\n",
    "        ''' Remove warping of image (change from \"overhead\"  back to \"perspective).\n",
    "        '''\n",
    "        return cv2.warpPerspective(image, self.perspectiveMatrixInv, (image.shape[1], image.shape[0]), flags=cv2.INTER_LINEAR)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Create and initalize a Transform object for our camera"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# glob to chessboard images\n",
    "chessboard_imgs_path = 'camera_cal\\calibration*.jpg'\n",
    "\n",
    "# chessboards are 9x6\n",
    "chessboard_nx = 9\n",
    "chessboard_ny = 6\n",
    "\n",
    "# Parameters to initialize camera perspective transformations.\n",
    "src_points  = np.float32([(140, 720), (550, 435), (680, 435), (1130, 720)])\n",
    "dest_points = np.float32([(290, 720), (290, 0), (990, 0), (990, 720)])\n",
    "\n",
    "transform = Transform()\n",
    "transform.calibrate_with_chessboards(chessboard_imgs_path, chessboard_nx, chessboard_ny)\n",
    "transform.init_perspective_transform(src_points, dest_points)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Image thresholding utility methods\n",
    "Various methods for binarizing an image based on thresholds.\n",
    "\n",
    "* sobel_thresh - for thresholding based on sobel gradient operations, including x-diretion, y-direction, magnitude and direction.\n",
    "* color_channel_thresh = for thresholding a single image channel (color channel) based on pixel range."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "def sobel_thresh(gray_img, sobel_kernel=3, x_thresh=(0, 255), y_thresh=(0,255), mag_thresh=(0, 255), dir_thresh=(0, np.pi/2)):\n",
    "    ''' Threshold an gray scale image based on the gradient (sobel operator)\n",
    "    \n",
    "    Args:\n",
    "        gray_img: gray image to perform sobel operation on and threshold.\n",
    "        sobel_kernel: size of the sobel kernel to use.  Default is 3.\n",
    "        x_thresh: lower/upper bound for x-direction threshold. Default is (0, 255), which is \"all\".\n",
    "        y_thresh: lower/upper bound for y-direction threshold. Default is (0, 255), which is \"all\".\n",
    "        mag_thresh: lower/upper bound for magnitude threshold. Default is (0,255), which is \"all\".\n",
    "        dir_thresh: lower/upper bound for gradient direction.  Default is (0, pi/2) which is \"all\".\n",
    "    \n",
    "    Returns:\n",
    "        4 binary images, one for each threshold.\n",
    "    '''\n",
    "    sobelx = np.copy(gray_img)\n",
    "    sobely = np.copy(gray_img)\n",
    "    \n",
    "    # Calculate gradient magnitude\n",
    "    sobelx = cv2.Sobel(sobelx, cv2.CV_64F, 1, 0, ksize=sobel_kernel) # Take the derivative in x\n",
    "    sobely = cv2.Sobel(sobely, cv2.CV_64F, 0, 1, ksize=sobel_kernel) # Take the derivative in y\n",
    "    \n",
    "    # x-direction absolute value, scaled, threshold\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",
    "    grad_x_binary = np.zeros_like(scaled_sobel)\n",
    "    grad_x_binary[(scaled_sobel >= x_thresh[0]) & (scaled_sobel <= x_thresh[1])] = 1\n",
    "    \n",
    "    # y-direction absolute value, scaled, threshold\n",
    "    abs_sobely = np.absolute(sobely) # Absolute y derivative to accentuate lines away from horizontal\n",
    "    scaled_sobel = np.uint8(255*abs_sobely/np.max(abs_sobely))\n",
    "    grad_y_binary = np.zeros_like(scaled_sobel)\n",
    "    grad_y_binary[(scaled_sobel >= y_thresh[0]) & (scaled_sobel <= y_thresh[1])] = 1\n",
    "    \n",
    "    # magnitude, scaled, threshold\n",
    "    sobel_mag = np.sqrt(sobelx ** 2 + sobely ** 2)\n",
    "    scaled_sobel = np.uint8(255*sobel_mag/np.max(sobel_mag))\n",
    "    mag_binary = np.zeros_like(scaled_sobel)\n",
    "    mag_binary[(scaled_sobel >= mag_thresh[0]) & (scaled_sobel <= mag_thresh[1])] = 1\n",
    "    \n",
    "    # gradient direction absolute value, scaled, threshold\n",
    "    absgraddir = np.arctan2(abs_sobely, abs_sobelx)\n",
    "    dir_binary = np.zeros_like(absgraddir)\n",
    "    dir_binary[(absgraddir >= dir_thresh[0]) & (absgraddir <= dir_thresh[1])] = 1\n",
    "    \n",
    "    return grad_x_binary, grad_y_binary, mag_binary, dir_binary\n",
    "\n",
    "\n",
    "def color_channel_thresh(color_channel, thresh=(0, 255)):\n",
    "    ''' Threshold a single channel (gray) image.\n",
    "    \n",
    "    Args:\n",
    "        color_channel: The single channel image to threshold.\n",
    "        thresh: lower/upper bound for pixel threshold. Default is (0, 255), which is \"all\".\n",
    "    \n",
    "    Returns:\n",
    "        Binary, thresholded image.\n",
    "    '''\n",
    "    binary = np.zeros_like(color_channel)\n",
    "    binary[(color_channel >= thresh[0]) & (color_channel <= thresh[1])] = 1\n",
    "    return binary\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Binarize image utility\n",
    "Binarizes the image using various techniques to be used to dectect lane lines.\n",
    "\n",
    "Combines the \"sobel_thresh\" results and the \"color_channel_thresh\" results to create binary mask.\n",
    "* Combines the intersection of the x and y sobel thresholds.\n",
    "* Combines the intersection magnitude and direction sobel thresholds.\n",
    "* Combines the union of the above 2 and the color channel threshold.\n",
    "\n",
    "Note: for the color channel, it is using the \"S\" channel after converting to HLS color space."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Thresholding hyper-parameters\n",
    "sobel_kernel_size = 9\n",
    "sobel_x_thresholds = (50, 200)\n",
    "sobel_y_thresholds = (0, 190)\n",
    "sobel_mag_thresholds = (50, 180)\n",
    "sobel_dir_thresholds = (0.7, 1.3)\n",
    "color_cannel_thresholds = (170, 250)\n",
    "\n",
    "\n",
    "def to_binary_mask(image):\n",
    "    \n",
    "    hls = cv2.cvtColor(image, cv2.COLOR_RGB2HLS).astype(np.float)\n",
    "    l_channel = hls[:,:,1]\n",
    "    s_channel = hls[:,:,2]\n",
    "    \n",
    "    # Apply each of the thresholding functions\n",
    "    gradx, grady, mag_binary, dir_binary = \\\n",
    "        sobel_thresh(l_channel, sobel_kernel_size, \\\n",
    "                     sobel_x_thresholds, sobel_y_thresholds, \\\n",
    "                     sobel_mag_thresholds, sobel_dir_thresholds)\n",
    "\n",
    "    channel_binary = color_channel_thresh(s_channel, color_cannel_thresholds)\n",
    "    \n",
    "    binary_mask = np.zeros_like(dir_binary)\n",
    "    binary_mask[((gradx == 1) & (grady == 1)) | ((mag_binary == 1) & (dir_binary == 1)) | (channel_binary == 1)] = 1\n",
    "    \n",
    "    return binary_mask"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Pixel/Meter conversion\n",
    "The Pixel/Meter conversion factors, and method to calculate \"radius of curvature\" in meters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "image_height_pixels = 720\n",
    "image_width_pixels = 1280\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",
    "def calc_radius_of_curvature(pixels_x, pixels_y):\n",
    "    coefficients = np.polyfit(pixels_y * ym_per_pix, pixels_x * xm_per_pix, 2)\n",
    "    return ((1 + (2 * coefficients[0] * image_height_pixels * ym_per_pix + coefficients[1])**2)**1.5) / np.absolute(2 * coefficients[0])\n",
    "\n",
    "def x_in_meters(x):\n",
    "    return x * xm_per_pix\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## \"Line\" class\n",
    "Encapsulates information about a \"Line\".\n",
    "\n",
    "A line is initialized by passing pixel locations to \"set_new_line_pixels\", which will fit a quadradic to those pixels.\n",
    "Does \"sanity\" checks on the passed in pixels, and will set \"detected\" = False if checks fail.\n",
    "Keeps a history of previous detected lines, the average being the \"best fit\"."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Number of previous \"fits\" to keep.\n",
    "max_fits_to_keep = 5\n",
    "\n",
    "# Minimum number of pixels that have to be passed for\n",
    "# a line to be considered \"detected\".\n",
    "min_pixels_per_line = 100\n",
    "\n",
    "# Minimum radius of curvature (in meters) a line can\n",
    "# have and be considered \"detected\".\n",
    "min_radius_of_curvature = 100 # meters\n",
    "\n",
    "class Line():\n",
    "    ''' Encapsulates a \"lane line\" (left or right).\n",
    "        \n",
    "        Pass pixel locations to \"set_new_line_pixels\" to\n",
    "        initialize (or re-initialize) a line.\n",
    "    '''\n",
    "    def __init__(self):\n",
    "        \n",
    "        # was the line detected in the last iteration?\n",
    "        self.detected = False\n",
    "        \n",
    "        # polynomial coefficients for the most recent fit\n",
    "        self.current_fit = None\n",
    "        \n",
    "        # polynomial coefficients of the previous n fits\n",
    "        self.prev_fits = []\n",
    "        \n",
    "        #radius of curvature of the line in meters\n",
    "        self.radius_of_curvature = None\n",
    "        \n",
    "        #x values for detected line pixels\n",
    "        self.allx = None\n",
    "        \n",
    "        #y values for detected line pixels\n",
    "        self.ally = None\n",
    "\n",
    "    def reset(self):\n",
    "        ''' Reset all line values to defaults (uninitialize).\n",
    "        '''\n",
    "        self.detected = False\n",
    "        self.current_fit = None\n",
    "        self.prev_fits = []\n",
    "        self.radius_of_curvature = None\n",
    "        self.allx = None\n",
    "        self.ally = None\n",
    "    \n",
    "    def set_not_detected(self):\n",
    "        ''' Set the \"detected\" flag to false (also remove \"current_fit\" coefficients)\n",
    "        '''\n",
    "        self.detected = False\n",
    "        self.current_fit = None\n",
    "    \n",
    "    def has_best_fit(self):\n",
    "        ''' Are there \"best fit\" coefficients?\n",
    "        \n",
    "            There will be a \"best fit\" if one or more lines have been detected. \n",
    "        \n",
    "        Returns:\n",
    "            True if there is a \"best fit\"\n",
    "        '''\n",
    "        return (len(self.prev_fits) > 0) or (self.current_fit is not None)\n",
    "    \n",
    "    def get_best_fit(self):\n",
    "        ''' Get the \"best fit\" which is the average of the current and\n",
    "            and previous n fit coefficients (for last n detected lines).\n",
    "        '''\n",
    "        # best fit is average of the last n fits\n",
    "        all_fits = self.prev_fits\n",
    "        if self.current_fit is not None:\n",
    "            if len(all_fits) > 0:\n",
    "                all_fits = np.concatenate([self.prev_fits, [self.current_fit]], axis=0)\n",
    "            else:\n",
    "                all_fits = [self.current_fit]\n",
    "        \n",
    "        if len(all_fits) > max_fits_to_keep:\n",
    "            all_fits = all_fits[1:]\n",
    "        \n",
    "        return np.mean(all_fits, axis=0)\n",
    "    \n",
    "    def x_using_best_fit(self, y_vals):\n",
    "        ''' Calculate the \"x\" values for the given \"y\" values using\n",
    "            the coefficients of the \"best fit\"\n",
    "        '''\n",
    "        if not self.has_best_fit():\n",
    "            return None\n",
    "        \n",
    "        coefficients = self.get_best_fit()\n",
    "        return (coefficients[0]*(y_vals**2) + coefficients[1]*y_vals + coefficients[2])\n",
    "    \n",
    "    def x_using_current_fit(self, y_vals):\n",
    "        ''' Calculate the \"x\" values for the given \"y\" values using\n",
    "            the coefficients of the \"current fit\"\n",
    "        '''\n",
    "        if self.current_fit == None:\n",
    "            return None\n",
    "        \n",
    "        return (self.current_fit[0]*(y_vals**2) + self.current_fit[1]*y_vals + self.current_fit[2])\n",
    "    \n",
    "    def set_new_line_pixels(self, pixel_x_vals, pixel_y_vals):\n",
    "        ''' Initialize the Line with the specified pixel locations.\n",
    "        \n",
    "            Will fit a quadradic to the pixel locations, the results being\n",
    "            stored as the \"current fit\".    If there are not a minimum\n",
    "            number of pixels specified, or the resuling cuve has too\n",
    "            much curvature, then the \"detected\" property will be set\n",
    "            to False, and there will be no \"current fit\".\n",
    "        '''\n",
    "        # save pevious fit to \"prev_fits\", if it was detected\n",
    "        if self.current_fit is not None:\n",
    "            if len(self.prev_fits) >= max_fits_to_keep:\n",
    "                self.prev_fits = self.prev_fits[1:]\n",
    "            self.prev_fits.append(self.current_fit)\n",
    "        \n",
    "        self.detected = False\n",
    "        self.current = None\n",
    "        if len(pixel_x_vals) >= min_pixels_per_line:\n",
    "            \n",
    "            rcurv = calc_radius_of_curvature(pixel_x_vals, pixel_y_vals)\n",
    "            \n",
    "            if rcurv >= min_radius_of_curvature:\n",
    "                self.detected = True\n",
    "                self.allx = pixel_x_vals\n",
    "                self.ally = pixel_y_vals\n",
    "                self.radius_of_curvature = rcurv\n",
    "                self.current_fit = np.polyfit(pixel_y_vals, pixel_x_vals, 2)\n",
    "        \n",
    "        return self.detected"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## \"Lane\" class\n",
    "Encapsulates information about a \"Lane\", which is composed of a left and right \"Line\".\n",
    "\n",
    "A lane is initialized by passing pixel locations for its left and right line to \"set_new_llane_pixels\".\n",
    "Does \"sanity\" checks to see if the lines make a valid \"lane\", will set \"detected\" = False if checks fail.\n",
    "Keeps a count of the number of times it failed to detect a lane."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# min/max lane width (pixels) to validate lanes\n",
    "min_lane_width = 500\n",
    "max_lane_width = 900\n",
    "\n",
    "class Lane():\n",
    "    ''' Encapsulates \"lane\", consiting of left and right \"line\".\n",
    "        \n",
    "        Pass pixel locations to \"set_new_lane_pixels\" to\n",
    "        initialize (or re-initialize) a lane.\n",
    "    '''\n",
    "    def __init__(self):\n",
    "        self.left_line = Line()\n",
    "        self.right_line = Line()\n",
    "        \n",
    "        self.detected = False\n",
    "        self.no_detect_count = 0\n",
    "        \n",
    "        \n",
    "    def __is_reasonable_lane__(self):\n",
    "        ''' Sanity check to see if current lines make a valid lane.'''\n",
    "        # were lines even detected?\n",
    "        if (not self.left_line.detected) and (not self.right_line.detected):\n",
    "            return False\n",
    "        \n",
    "        # plot the left and right line\n",
    "        ploty = np.linspace(0, image_height_pixels - 1, image_height_pixels)\n",
    "        left_fitx  = self.left_line.x_using_best_fit(ploty)\n",
    "        right_fitx = self.right_line.x_using_best_fit(ploty)\n",
    "        \n",
    "        # is the left line on the left?\n",
    "        if left_fitx[image_height_pixels - 1] > ((image_width_pixels//2)):\n",
    "            return False\n",
    "        \n",
    "        # is the right line on the right?\n",
    "        if right_fitx[image_height_pixels - 1] < ((image_width_pixels//2)):\n",
    "            return False\n",
    "        \n",
    "        # use difference between right and left line\n",
    "        # x positions as the \"lane width\"\n",
    "        diff_fitx = right_fitx - left_fitx\n",
    "        \n",
    "        # is the mimimum width a reasonable lane width?\n",
    "        min_fitx = np.min(diff_fitx)\n",
    "        if (min_fitx < min_lane_width) or (min_fitx > max_lane_width):\n",
    "            return False\n",
    "        \n",
    "        # is the maximum width a reasonable lane width?\n",
    "        max_fitx = np.max(diff_fitx)\n",
    "        if(max_fitx < min_lane_width) or (max_fitx > max_lane_width):\n",
    "            return False\n",
    "        \n",
    "        return True\n",
    "    \n",
    "    def reset(self):\n",
    "        ''' Reset all land (and line) values to defaults (uninitialize).\n",
    "        '''\n",
    "        self.detected = False\n",
    "        self.no_detect_count = 0\n",
    "        self.left_line.reset()\n",
    "        self.right_line.reset()\n",
    "    \n",
    "    def has_best_fit(self):\n",
    "        ''' Check if both left and right line have a \"best fit\"\n",
    "        '''\n",
    "        return (self.left_line.has_best_fit()) and (self.right_line.has_best_fit())\n",
    "    \n",
    "    def get_radius_of_curvature(self):\n",
    "        ''' Return radius of curvature in meters (average of left/right line)\n",
    "        '''\n",
    "        if not self.has_best_fit():\n",
    "            return None\n",
    "        \n",
    "        return (self.left_line.radius_of_curvature + self.right_line.radius_of_curvature) / 2\n",
    "    \n",
    "    def get_offset_from_center(self):\n",
    "        ''' How from off from the center of the lane is the vehicle (in meters)\n",
    "        '''\n",
    "        if not self.has_best_fit():\n",
    "            return None\n",
    "        \n",
    "        x_left = self.left_line.x_using_best_fit(image_height_pixels)\n",
    "        x_right = self.right_line.x_using_best_fit(image_height_pixels)\n",
    "        x_center = x_left + ( (x_right - x_left) // 2)\n",
    "        x_vehicle_offset = (image_width_pixels //2) - x_center\n",
    "        \n",
    "        return x_in_meters(x_vehicle_offset)\n",
    "    \n",
    "    def set_new_lane_pixels(self, left_pixels, right_pixels):\n",
    "        ''' Initialize a lane passing left/right line pixels.\n",
    "        \n",
    "            Will check if the resulting lines make up a valid lane, and\n",
    "            if they are not, will set \"detected\" = False.\n",
    "            \n",
    "            Also keeps a count of the number of times \"not detected\" in a row.\n",
    "        '''\n",
    "        left_detected =  self.left_line.set_new_line_pixels(left_pixels[0], left_pixels[1])\n",
    "        right_detected = self.right_line.set_new_line_pixels(right_pixels[0], right_pixels[1])\n",
    "        \n",
    "        self.detected = False\n",
    "        if (left_detected) and (right_detected):\n",
    "            self.detected = self.__is_reasonable_lane__()\n",
    "        \n",
    "        if not self.detected:\n",
    "            self.left_line.set_not_detected()\n",
    "            self.right_line.set_not_detected()\n",
    "            self.no_detect_count += 1\n",
    "        else:\n",
    "            self.no_detect_count = 0\n",
    "    \n",
    "        return self.detected\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Method to find line pixels (from scratch)\n",
    "Find left/right line pixel whithout any prior line information"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# number of vertical slices to search for lines\n",
    "nwindows = 9\n",
    "\n",
    "# margin (left/right) of the pixel search window\n",
    "window_margin = 100\n",
    "\n",
    "# number of pixels required to recenter the search window\n",
    "minpix_for_new_centroid = 50\n",
    "\n",
    "\n",
    "def find_lane_pixels_initial(binary_warped):\n",
    "    \n",
    "    nonzero = binary_warped.nonzero()\n",
    "    nonzeroy = np.array(nonzero[0])\n",
    "    nonzerox = np.array(nonzero[1])\n",
    "    \n",
    "    # Take a histogram of the bottom third of the image\n",
    "    histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0)\n",
    "    \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",
    "    # Set height of windows\n",
    "    window_height = np.int(binary_warped.shape[0]/nwindows)\n",
    "    \n",
    "    # Current positions to be updated for each window\n",
    "    leftx_current = leftx_base\n",
    "    rightx_current = rightx_base\n",
    "    \n",
    "    left_lane_inds = []\n",
    "    right_lane_inds = []\n",
    "    \n",
    "    # Step through the windows one by one\n",
    "    for window in range(nwindows):\n",
    "        \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_y_center = (win_y_high - win_y_low)//2;\n",
    "        \n",
    "        win_xleft_center = leftx_current\n",
    "        win_xright_center = rightx_current\n",
    "        \n",
    "        win_xleft_low = win_xleft_center - window_margin\n",
    "        win_xleft_high = win_xleft_center + window_margin\n",
    "        win_xright_low = win_xright_center - window_margin\n",
    "        win_xright_high = win_xright_center + window_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) & (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]\n",
    "        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]\n",
    "        \n",
    "        # Append these indices to the lists\n",
    "        left_lane_inds.append(good_left_inds)\n",
    "        right_lane_inds.append(good_right_inds)\n",
    "        \n",
    "        if len(good_left_inds) > minpix_for_new_centroid:\n",
    "            leftx_current = np.int(np.mean(nonzerox[good_left_inds]))\n",
    "        if len(good_right_inds) > minpix_for_new_centroid:        \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",
    "    left_x = nonzerox[left_lane_inds]\n",
    "    left_y = nonzeroy[left_lane_inds]\n",
    "    \n",
    "    right_x = nonzerox[right_lane_inds]\n",
    "    right_y = nonzeroy[right_lane_inds] \n",
    "    \n",
    "    return (left_x, left_y), (right_x, right_y)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Method to find line pixels (using previous line)\n",
    "Search around the previously found line for new line pixels"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Margin (left/right) around the \"best fit\" line to search for pixels\n",
    "best_fit_margin = 50\n",
    "\n",
    "def find_lane_pixels_using_previous(binary_warped, lane):\n",
    "    nonzero = binary_warped.nonzero()\n",
    "    nonzeroy = np.array(nonzero[0])\n",
    "    nonzerox = np.array(nonzero[1])\n",
    "    \n",
    "    left_x_centers = lane.left_line.x_using_best_fit(nonzeroy)\n",
    "    left_lane_inds = ((nonzerox >  (left_x_centers - best_fit_margin)) & (nonzerox < (left_x_centers + best_fit_margin)))\n",
    "    \n",
    "    right_x_centers = lane.right_line.x_using_best_fit(nonzeroy)\n",
    "    right_lane_inds = ((nonzerox > (right_x_centers - best_fit_margin)) & (nonzerox < (right_x_centers + best_fit_margin)))\n",
    "    \n",
    "    left_x = nonzerox[left_lane_inds]\n",
    "    left_y = nonzeroy[left_lane_inds]\n",
    "    \n",
    "    right_x = nonzerox[right_lane_inds]\n",
    "    right_y = nonzeroy[right_lane_inds] \n",
    "    \n",
    "    return (left_x, left_y), (right_x, right_y)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Method to find a lane in the specifed binary image\n",
    "Must pass in a \"Lane\" object, which is contextual \n",
    "(should always pass the same \"Lane\" object for subsequent frames in a video)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# maximum number of times \"no lane detected\" can happen before\n",
    "# resetting and search for lanes from scratch.\n",
    "max_no_detects = 5\n",
    "\n",
    "def find_lane(binary_warped, lane):\n",
    "    ''' Find a \"lane\" in the specified binary image.\n",
    "    \n",
    "    Args:\n",
    "        binary_warped: a binary image of just the lines of an image that has been perspective \"warped\" (top-down image).\n",
    "        lane: a Lane object, holds any past lanes that were found, and is where the currently found land is stored.\n",
    "    \n",
    "    Results:\n",
    "        lane: returns the same lane object passed in, populated with the found lane (if any).\n",
    "    '''\n",
    "    used_previous_fit = False\n",
    "    \n",
    "    if lane.has_best_fit():\n",
    "        # find left/right lines using previous best fit.\n",
    "        used_previous_fit = True\n",
    "        left_pixels, right_pixels = find_lane_pixels_using_previous(binary_warped, lane)\n",
    "    else:\n",
    "        # Find left and right line from scratch\n",
    "        left_pixels, right_pixels = find_lane_pixels_initial(binary_warped)\n",
    "    \n",
    "    lane.set_new_lane_pixels(left_pixels, right_pixels)\n",
    "    \n",
    "    if (not lane.detected) and (lane.no_detect_count >= max_no_detects):\n",
    "        # too many no-detects, start fresh\n",
    "        lane.reset()\n",
    "        if used_previous_fit:\n",
    "            #last attempt was via previous fit, try again from scratch\n",
    "            left_pixels, right_pixels = find_lane_pixels_initial(binary_warped)\n",
    "            lane.set_new_lane_pixels(left_pixels, right_pixels)\n",
    "    \n",
    "    return lane"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Create color image with lane filled in on black background.\n",
    "To be overlayed on top of the original image to highlight the lane."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def fill_lane(warped_binary_mask, lane):\n",
    "    \n",
    "    warp_zero = np.zeros_like(warped_binary_mask).astype(np.uint8)\n",
    "    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))\n",
    "    \n",
    "    if not lane.has_best_fit():\n",
    "        return color_warp\n",
    "    \n",
    "    ploty = np.linspace(0, warped_binary_mask.shape[0]-1, warped_binary_mask.shape[0])\n",
    "    \n",
    "    left_fitx  = lane.left_line.x_using_best_fit(ploty)\n",
    "    right_fitx = lane.right_line.x_using_best_fit(ploty)\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",
    "    return color_warp\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Closure to return a \"lane finder\"\n",
    "The \"lane finder\" closure initializes a \"Lane\" object when called, and ensures all subsequent calls to \"find lane\" will use the same \"Lane\" object."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_lane_finder():\n",
    "    \n",
    "    lane = Lane()\n",
    "    \n",
    "    def process_image(image):\n",
    "        img_undistort = transform.remove_distortion(image)\n",
    "        binary_mask = to_binary_mask(img_undistort)\n",
    "        \n",
    "        warped_binary_mask = transform.warp_perspective(binary_mask)\n",
    "        \n",
    "        find_lane(warped_binary_mask, lane)\n",
    "        \n",
    "        # Combine the result with the original image\n",
    "        if lane.has_best_fit():\n",
    "            lane_colorized = fill_lane(warped_binary_mask, lane)\n",
    "            lane_colorized = transform.unwarp_perspective(lane_colorized)\n",
    "            \n",
    "            # Combine the lane color with the original image\n",
    "            final_img = cv2.addWeighted(img_undistort, 1, lane_colorized, 0.3, 0)\n",
    "            curvature_msg = \"Radius of Curvature is {0:.0f}(m)\".format(lane.get_radius_of_curvature())\n",
    "            \n",
    "            offset = lane.get_offset_from_center()\n",
    "            if offset < 0:\n",
    "                offset_msg = \"Vehicle is {0:.3f}m left of center\".format(abs(offset))\n",
    "            elif offset > 0:\n",
    "                offset_msg = \"Vehicle is {0:.3f}m right of center\".format(offset)\n",
    "            else:\n",
    "                offset_msg = \"Vehicle is centered\"\n",
    "            \n",
    "            cv2.putText(final_img, curvature_msg, (100,100), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 255, 255), 1)\n",
    "            cv2.putText(final_img, offset_msg, (100,200), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 255, 255), 1)\n",
    "            \n",
    "        else:\n",
    "            final_img = img_undistort\n",
    "            cv2.putText(final_img, \"No Lane Detected\", (100,100), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 255, 255), 1)\n",
    "        \n",
    "        \n",
    "        \n",
    "        return final_img\n",
    "    \n",
    "    return process_image\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Process the video"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[MoviePy] >>>> Building video project_video_out.mp4\n",
      "[MoviePy] Writing video project_video_out.mp4\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|█████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████████▉| 1260/1261 [04:16<00:00,  5.17it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[MoviePy] Done.\n",
      "[MoviePy] >>>> Video ready: project_video_out.mp4 \n",
      "\n"
     ]
    }
   ],
   "source": [
    "from moviepy.editor import VideoFileClip\n",
    "from IPython.display import HTML\n",
    "\n",
    "video_output = 'project_video_out.mp4'\n",
    "clip1 = VideoFileClip(\"project_video.mp4\")\n",
    "video_clip = clip1.fl_image(get_lane_finder()) #NOTE: this function expects color images!!\n",
    "video_clip.write_videofile(video_output, audio=False)\n",
    "clip1.__del__()\n",
    "video_clip.__del__()\n"
   ]
  }
 ],
 "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
}