<repo_name>erik-rosen/self-driving-car-project-lane-finding<file_sep>/README.md
## Advanced lane finding

![Alt Text](./output_images/output_project.gif)

**Advanced Lane Finding Project**

The goals / steps of this project are the following:

* Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.
* Apply a distortion correction to raw images.
* Apply a perspective transform to rectify binary image ("birds-eye view").
* Use color transforms, gradients, etc., to create a thresholded binary image that marks lane lines in an image.
* Detect lane pixels and fit to find the lane boundary.
* Determine the curvature of the lane and vehicle position with respect to center.
* Warp the detected lane boundaries back onto the original image.
* Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.

[//]: # (Image References)

[image1]: ./output_images/undistort_output.png "Undistorted"
[image2]: ./output_images/undistort_output2.png "Road Transformed"
[image3]: ./output_images/straight_lines2.jpg "Binary example"
[image4]: ./output_images/straight_lines1.jpg "Birdseye view of road, top right"
[image5]: ./output_images/test1.jpg "Processed image"
[image6]: ./output_images/test2.jpg "Processed image - good performance"
[image7]: ./output_images/test5.jpg "Processed image - poor performance"
[image8]: ./output_images/test6.jpg "Processed image - poor performance"
[image9]: ./output_images/test4.jpg "Processed image - good performance"
[video1]: ./project_video_dog.mp4 "Video"


### Camera Calibration

#### 1. Briefly state how you computed the camera matrix and distortion coefficients. Provide an example of a distortion corrected calibration image.

The code for this step is contained in the first code cell of the IPython notebook located in `./camera_calibration.ipynb`

I start by preparing "object points", which will be the (x, y, z) coordinates of the chessboard corners in the world. I assume the chessboard is fixed on the (x, y) plane at z=0, such that the object points are the same for each calibration image. `objp` will therefore be a replicated array of coordinates, and `objpoints` will be appended with a copy of it every time we detect all chessboard corners in a test image.  `imgpoints` will be appended with the (x, y) pixel position of each of the corners in the image plane with each successful chessboard detection.  

I load in the images in the `./camera_cal/` directory in a for-loop, and use OpenCV's `findChessboardCorners()` to extract the corners in the chessboard in the image. If a chessboard is found, I apply OpenCV's `cornerSubPix()` function which refines the position of the corner to subpixel accuracy.

I then used the output `objpoints` and `imgpoints` to compute the camera calibration and distortion coefficients using the `cv2.calibrateCamera()` function.  I applied this distortion correction to the test image using the `cv2.undistort()` function and obtained this result: 

![alt text][image1]

I serialize the computed camera matrix and the distortion coefficients into a pickle file that I can load in when I run the lanefinding pipeline. This saves me from having to run the calibration code (extract corners, compute the camera matrix and distortion coefficients) everytime I run the pipeline.

### Pipeline (single images)

#### 1. Provide an example of a distortion-corrected image.

In "./lane_finder.ipynb" I start by loading in the camera matrix and distortion coefficents that we saved earlier in the pickle file. I also define the perspective transfrom (more on this later). All of these are passed to the constructor of my instatiation of a `LaneFinder` object, `l`. The LaneFinder object is defined in `./lanefinder.py` and contains all methods used in the pipeline, the main one being `findLanes()`, defined on line 105. This method takes an input frame, extracts the lane lines from that frame, computes their parameters, and outputs a frame illustrating the intermediary steps in the pipeline, as well as the estimated curvature of the lane lines extracted, and the offset of the car from the center oof the lane.

The first step in the `findLanes()` method is correcting the raw input image from camera distortion using openCV's `undistort()` function, which takes the camera matrix and the distortion coefficients which were passed to the constructor of the `LaneFinder` object. The result is shown below.

![alt text][image2]

#### 2. Describe how (and identify where in your code) you performed a perspective transform and provide an example of a transformed image.

The second step in the `findLanes()` method is to compute a "birdseye" view of the road using OpenCV's `warpPerspective()` method. See line 112 in `lanefinder.py`. The perspective tranform matrix used is passed to the constructor of the `LaneFinder` object. 

In our case this matrix is created by manually defining a set of points in the undistorted image. Each point was picked such that the top side and bottom sides of the resulting shape are horizontal, and each point is on the outside of a lane line on the road. The top side points are positioned such that the lane lines are on the verge of being difficult to distingish around that distance, and the bottom side points are picked such that the bottom side is at the bottom of the original image. 

The destination points where selected such that: 1) the resulting shape was perfectly rectangular, 2) there were no "undefined"/dark sections in the resulting view and 3) the car bonnet was not visible.

This resulted in the following source and destination points:

| Source        | Destination   | 
|:-------------:|:-------------:| 
| 578, 460      | 280, 0        | 
| 702, 460      | 1000, 0       |
| 1100, 720     | 1000, 748     |
| 180, 720      | 280, 748      |

You can see this in line 23-25 in the first cell of `./lane_finder.ipynb`.

I verified that my perspective transform was working as expected by checking that the resulting lane lines in the transformed image were parallel (see top right for birdseye view image tranformed from the original "driver seat" view):

![alt text][image4]

#### 3. Describe how (and identify where in your code) you used color transforms, gradients or other methods to create a thresholded binary image.  Provide an example of a binary image result.

The next step in the `findLanes()` method is to compute a lane segmented binary image. See line 116 of `./lanefinder.py`, which calls the method `laneLineSegmentationLaplace()`.

I experimented with a number of techniques to effectively segment lane lines. The one  which performed best can be found in rows 205-222 of `./lanefinder.py`, `laneLineSegmentationLaplace()`.

This method takes the birdseye view image of the road, and creates a weighted image from the S-channel and grayscale version of the input frame. This accentuates both the yellow and the white lane lines in the resulting grayscale image well. See the image in the top left in the figure below for the resulting output.

Since the width of the lane lines are known, I apply a Laplacian filter with a kernel size which best corresponds to the lane line width (i.e. maximizes it's response when applied to a lane line). To avoid having to use a very large kernel (computationally expensive), I downsample the input images to a tenth of its original size. This allows me to use a much smaller kernel and get the same effect. 

The magnitude of the Laplacian filter output is sensitive to lighting conditions. To mitigate for that when thresholding, I linearly scale the output such that the minimum of the output is set to 0, and the maximum of the output is 255. I assume that if a road lane is present, this will likely result in the minimum output in the image. See the second image from the top left in the figure below for the resulting Laplacian filter output.

I then apply a threshold to the scaled Laplacian filter output to produce a binary image (in our case each pixel is either 0 or 255). See the third image from the top left in the figure below for the resulting output.

![alt text][image3]

#### 4. Describe how (and identify where in your code) you identified lane-line pixels and fit their positions with a polynomial?

The next step in the `findLanes()` method is to find the actual lane pixels in the binary image produced in the previous step. See lines 120-127 in `lanefinder.py`.

Depending on whether the lane line was confidently found in the prior frame, we search around the previously best fit polynomial using `search_around_poly()`, if not, we use sliding windows `find_lane_pixels_sliding_window_one_side()` to identify lane pixels from the thresholded image. These two methods are defined in lines 388-452 and 308-386 respectively in `lanefinder.py`. They return the identified lane line pixels and an image marking the effective lane pixel search areas.

The identified laneline pixels are passed to the `updateFit()` method belonging to the Line objects (instantiated as `leftLane` and `rightLane` belonging to the `LaneFinder` object).

This method can be found on lines 35-95 in `lanefinder.py`. This function first fits a second order polynomial to the the identified lane pixels.

If the number of identified lane line pixels are too few, if the radius of curvature of the interpolated points is lower than 60 meters, and if the difference between the previous best fit is too great, we reject the fit and mark the line as currently undetected.

If the detection is accepted, we update the best fit coefficients. The best fit coefficients are computed as the average of the last 5 accepted fits.

![alt text][image8]

#### 5. Describe how (and identify where in your code) you calculated the radius of curvature of the lane and the position of the vehicle with respect to center.

After updating the best fit coefficients in `updateFit()`, the radius of curvature and offset of the lane line from the vehicle centre is computed. See lines 87-93 in `lanefinder.py` The position of the vehicle relative to the center of the lane bounded by the lane lines is computed from the offset estimate of the left and right lane line relative to the center of the car on line 142 in `lanefinder.py`.

These metrics can all be seen in the image text overlay above, and the code to draw this overlay is defined on lines 137-145.

#### 6. Provide an example image of your result plotted back down onto the road such that the lane area is identified clearly.

As part of `updateFit()` that was run earlier, we store the points of the curve of the best fit second order polynomial the `Line` objects. The next step in the `findLanes()` method is to draw the lane line curves and the lane polygon on the birdseye view image. This is done on lines 150-154 and 166 in `lanefinder.py`. The result is the top right image seen in the figure below.

On line 155 we use the inverse perspective transform which we computed in the constructor of the LaneFinder object to warp the lane lines and lane overlays to how they look from the "driver view". On line 166, we blend the road-warped color overlay with the undistorted "driver view" image, resulting in the bottom image seen in the figure below.

![alt text][image6]

---

### Pipeline (video)


Here's a [link to my video result](./output_project_video_dog.mp4)

---

### Discussion

#### 1. Briefly discuss any problems / issues you faced in your implementation of this project.  Where will your pipeline likely fail?  What could you do to make it more robust?

The lane line segmentation of the current pipeline does not perform very well when the contrast between road and lane line marking is small, when the lane line is occluded or missing from the frame, when part of the road is in shadow, there are speckles on the road, or the color of the road changes - see below for examples where the pipeline fails or does not perform well:

![alt text][image5]
![alt text][image9]
![alt text][image7]

If I were to take this project futher, I would focus on improving the lane line segmentation part of the pipeline, likely exploring training and applying a convolutional neural net for the lane line segmentation task, which is the state of the art at the time of writing: https://paperswithcode.com/paper/learning-lightweight-lane-detection-cnns-by  

In order for the lane line finder to be useful for autonomous driving, it will need to be running with a sufficiently high frame rate throughput - the current framerate throughput of the implementation is likely too low for practical application. Improving the performance of the pipeline would be the second thing I would focus on.
<file_sep>/lanefinder.py
import numpy as np
import cv2

# Define a class to receive the characteristics of each line detection
class Line():
    def __init__(self, side):
        # was the line detected in the last iteration?
        self.side = side
        self.detected = False  
        # x values of the last n fits of the line
        self.recent_xfitted = [] 
        #points of the best fitted line over the last n iterations
        self.bestpts = np.int32( [ np.array([np.transpose(np.vstack(([], [])))])])    
        #polynomial coefficients averaged over the last n iterations
        self.best_fit = np.array([0,0,0], dtype='float')  
        #polynomial coefficients for the most recent fit
        self.current_fit = np.array([0,0,0], dtype='float') 
        #radius of curvature of the line in some units
        self.radius_of_curvature = None
        #distance in meters of vehicle center from the line
        self.line_base_pos = None
        #difference in fit coefficients between last and new fits
        self.diffs = np.array([0,0,0], dtype='float') 
        #x values for detected line pixels
        self.allx = None  
        #y values for detected line pixels
        self.ally = None
        #number of good fits currently averaged over - will never exceed self.average_over_n_fits
        self.number_of_fits = 0
        self.average_over_n_fits = 5
        # Define conversions in x and y from pixels space to meters
        self.ym_per_pix = 30/720 # meters per pixel in y dimension
        self.xm_per_pix = 3.7/700 # meters per pixel in x dimension
        
    def updateFit(self, allx, ally):
        self.allx = allx
        self.ally = ally
        
        # Initial sanity check - if the number of points found are too few, skip
        if allx.size<3: 
            self.detected = False 
            return None
        
        # Fit a second order polynomial to the points
        self.current_fit = np.polyfit(ally,allx,2)
        # also fit one that is scaled to meters
        current_fit_meters = np.polyfit(ally*self.ym_per_pix, allx*self.xm_per_pix, 2)
        
        y_eval=720*self.ym_per_pix
        curverad = (1+(2*current_fit_meters[0]*y_eval+current_fit_meters[1])**2)**1.5 / (2*abs(current_fit_meters[0]))
        if self.number_of_fits > 0:
            self.diffs = np.absolute(self.current_fit - self.best_fit)
        
        # Sanity checks
        if (curverad<60):
            self.detected = False
            return None
            
        if (np.sum(self.diffs)>1000):
            self.detected = False
            return None
        
        self.detected = True
        
        if (self.number_of_fits < self.average_over_n_fits):
            self.number_of_fits=self.number_of_fits + 1
        
        #Average over n last fits - if there were none, adjust accordingly
        weight_current_fit = 1 / self.number_of_fits
        weight_of_historical_fit = 1 - weight_current_fit
        
        self.best_fit = [self.current_fit[0]*weight_current_fit + self.best_fit[0]*weight_of_historical_fit,
                    self.current_fit[1]*weight_current_fit + self.best_fit[1]*weight_of_historical_fit,
                    self.current_fit[2]*weight_current_fit + self.best_fit[2]*weight_of_historical_fit]
        
        # Generate x values for plotting
        ploty = np.linspace(0, 720-1, 720 )
        bestx = self.best_fit[0]*ploty**2 + self.best_fit[1]*ploty + self.best_fit[2]
        
        # Transform into points for plotting
        if self.side == 'left':
            self.bestpts = np.int32( [ np.array([np.transpose(np.vstack((bestx, ploty)))])])
        else:
            self.bestpts = np.int32( [ np.array( [ np.flipud( np.transpose( np.vstack( [bestx, ploty] ) ) ) ] ) ] )
            
        # Update radius of curvature
        best_fit_meters = np.polyfit(ploty*self.ym_per_pix, bestx*self.xm_per_pix, 2)
        y_eval=720*self.ym_per_pix
        self.radius_of_curvature = (1+(2*best_fit_meters[0]*y_eval+best_fit_meters[1])**2)**1.5 / (2*abs(best_fit_meters[0]))
        
        # Update base offset from centre - TODO: Clean up with parameters
        self.line_base_pos = (self.best_fit[0]*720**2 + self.best_fit[1]*720 + self.best_fit[2] - 1280/2) * self.xm_per_pix
        
        return None
        
class LaneFinder():
    def __init__(self,cameraMatrix,distCoeffs,perspectiveTransformMatrix):
        self.cameraMatrix = cameraMatrix
        self.distCoeffs = distCoeffs
        self.perspectiveTransformMatrix = perspectiveTransformMatrix
        self.inversePerspectiveTransform = np.linalg.inv(perspectiveTransformMatrix) 
        self.leftLane = Line('left')
        self.rightLane = Line('right')
        
    def findLanes(self, frame):
        frameSize = (frame.shape[1],frame.shape[0])
        
        #Undistort image
        frame = cv2.undistort(frame, self.cameraMatrix, self.distCoeffs, None, self.cameraMatrix)
        
        #Get the birdseye view:
        birdsEyeImage = cv2.warpPerspective(frame, self.perspectiveTransformMatrix, frameSize, flags=cv2.INTER_LINEAR)
        
        #Perform Lane Segmentation:
        #grayscale, abs_sobelx, thresh_sobel, extracted_lines = self.laneLineSegmentation(birdsEyeImage)
        grayscale, laplacian, extracted_lines = self.laneLineSegmentationLaplace(birdsEyeImage)
        
        #Find lane pixels
        #leftx, lefty, rightx, righty, extracted_lane_pixels = self.find_lane_pixels_sliding_window(extracted_lines)
        if (self.leftLane.detected):
            leftx, lefty, left_img = self.search_around_poly(extracted_lines.copy(),self.leftLane)
        else: 
            leftx, lefty, left_img = self.find_lane_pixels_sliding_window_one_side(extracted_lines.copy(),self.leftLane)
        if (self.rightLane.detected):
            rightx, righty, right_img = self.search_around_poly(extracted_lines.copy(),self.rightLane)
        else: 
            rightx, righty, right_img = self.find_lane_pixels_sliding_window_one_side(extracted_lines.copy(),self.rightLane)
        
        
        extracted_lane_pixels = np.hstack((left_img, right_img))
        
        #Update lines
        self.leftLane.updateFit(leftx,lefty)
        self.rightLane.updateFit(rightx,righty)
        font = cv2.FONT_HERSHEY_SIMPLEX
        cv2.putText(frame,'L curv: '+ str(self.leftLane.radius_of_curvature) ,(900,30), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
        cv2.putText(frame,'R curv: '+ str(self.rightLane.radius_of_curvature) ,(900,60), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
        cv2.putText(frame,'L offset: '+ str(self.leftLane.line_base_pos) ,(900,90), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
        cv2.putText(frame,'R offset: '+ str(self.rightLane.line_base_pos) ,(900,120), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
        if (self.leftLane.number_of_fits>0 and self.rightLane.number_of_fits>0 ):
            center_offset = (self.leftLane.line_base_pos+self.rightLane.line_base_pos)*0.5
        else:
            center_offset = 0
        cv2.putText(frame,'Offset in Lane: '+ str(center_offset) ,(900,150), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
        #cv2.putText(frame,'L diffs: '+ str(np.sum(self.leftLane.diffs)) ,(900,180), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
        #cv2.putText(frame,'R diffs: '+ str(np.sum(self.rightLane.diffs)) ,(900,210), font, 1, (255, 255, 255), 2, cv2.LINE_AA)
        
        #Draw best averaged best fit lanes
        poly = np.hstack((self.leftLane.bestpts[0], self.rightLane.bestpts[0]))
        color_overlay = np.zeros_like(frame)
        cv2.polylines(color_overlay, self.leftLane.bestpts, False, color=[255,0,0], thickness=30, lineType=cv2.LINE_AA)
        cv2.polylines(color_overlay, self.rightLane.bestpts, False, color=[0,0,255], thickness=30, lineType=cv2.LINE_AA)
        cv2.fillPoly(color_overlay, np.array([poly], dtype=np.int32), (0,255, 0))
        warped_color_overlay = cv2.warpPerspective(color_overlay, self.inversePerspectiveTransform, (frame.shape[1],frame.shape[0]), flags=cv2.INTER_NEAREST)
        
        
        #Stitch together images
        processingrow = np.hstack((
                                   cv2.cvtColor(grayscale,cv2.COLOR_GRAY2RGB), 
                                   #cv2.cvtColor(abs_sobelx,cv2.COLOR_GRAY2RGB), 
                                   #cv2.cvtColor(thresh_sobel,cv2.COLOR_GRAY2RGB),
                                   cv2.cvtColor(laplacian,cv2.COLOR_GRAY2RGB),
                                   cv2.cvtColor(np.uint8(extracted_lines),cv2.COLOR_GRAY2RGB),
                                   extracted_lane_pixels,
                                   cv2.addWeighted(birdsEyeImage, 1, color_overlay, 0.3, 0,-1)                            
                                  ))
        processingrow = cv2.resize(processingrow, (frameSize[0],frameSize[1]//4), interpolation = cv2.INTER_AREA)
        result = np.vstack((processingrow,cv2.addWeighted(frame, 1, warped_color_overlay, 0.3, 0,-1)))
        return result
    
    def laneLineSegmentation(self, birdsEyeImage):
        # S-channel is good at finding yellow lines and is largely invariant to lighting conditions
        # S-channel isn't great at finding the white lines though, so meld it together with
        # a greyscale image to find both white and yellow lines
        sChannel = cv2.cvtColor(birdsEyeImage,cv2.COLOR_RGB2HSV)[:,:,1]
        gray = cv2.cvtColor(birdsEyeImage,cv2.COLOR_RGB2GRAY)
        weighted = cv2.addWeighted(sChannel, 1, gray, 1, 0,-1)
        
        # Extract edges of the vertical lines
        sobelx = cv2.Sobel(weighted, cv2.CV_32F, 1, 0, 15)
        # Take the absolute value of the x gradients
        abs_sobelx = np.absolute(sobelx)
        
        # Threshold gradients
        thresh_sobel = np.zeros_like(abs_sobelx)
        thresh_sobel[(abs_sobelx > 40)] = 1
       
        # We know that lies are 20-40 pixels wide. By dilating the thresholded edges, we join the edges together
        # We then erode the dilated lines by more than that such that any edges that are not within a certain 
        # distance of another edge will be removed.
        #dilation_size = 40
        #erosion_size = 70
        # Create structure elements
        #dilationStructure = cv2.getStructuringElement(cv2.MORPH_RECT, (dilation_size, 1))
        #erosionStructure = cv2.getStructuringElement(cv2.MORPH_RECT, (erosion_size, 1))
        # Apply morphology operations
        #dilated = cv2.dilate(thresh_sobel, dilationStructure)
        #eroded = cv2.erode(dilated, erosionStructure)
        
        #extracted_lines = eroded * 255
        #return weighted, abs_sobelx, thresh_sobel*255, extracted_lines
        return weighted, abs_sobelx, thresh_sobel*255, thresh_sobel*255
    
    
    def laneLineSegmentationLaplace(self, birdsEyeImage):
        size = (birdsEyeImage.shape[1],birdsEyeImage.shape[0])
        downsample = cv2.resize(birdsEyeImage, (birdsEyeImage.shape[0]//10,birdsEyeImage.shape[1]//10), interpolation = cv2.INTER_AREA)
        sChannel = cv2.cvtColor(downsample,cv2.COLOR_RGB2HSV)[:,:,1]
        gray = cv2.cvtColor(downsample,cv2.COLOR_RGB2GRAY)
        weighted = cv2.addWeighted(sChannel, 1, gray, 1, 0,-1)
        #laplacian = cv2.filter2D(weighted,cv2.CV_32F,kernel)
        
        
        laplacian = cv2.Laplacian( weighted, ddepth = cv2.CV_32F, ksize=7 )
        l_min = np.amin(laplacian)
        l_max = np.amax(laplacian)
        laplacian = np.uint8((laplacian - l_min) * 255 / (l_max - l_min))
        thresh_laplacian = np.zeros_like(laplacian)
        thresh_laplacian[(laplacian < 100)] = 255
        # TODO: Implement
      
        return cv2.resize(weighted,size,interpolation = cv2.INTER_NEAREST), cv2.resize(laplacian,size,interpolation = cv2.INTER_NEAREST), cv2.resize(thresh_laplacian,(size),interpolation = cv2.INTER_NEAREST)
    
    
    def find_lane_pixels_sliding_window(self, binary_warped):
        # Take a histogram of the bottom half of the image
        histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0)
        # Create an output image to draw on and visualize the result
        out_img = np.dstack((binary_warped, binary_warped, binary_warped))
        # Find the peak of the left and right halves of the histogram
        # These will be the starting point for the left and right lines
        midpoint = np.int(histogram.shape[0]//2)
        leftx_base = np.argmax(histogram[:midpoint])
        rightx_base = np.argmax(histogram[midpoint:]) + midpoint
    
        # HYPERPARAMETERS
        # Choose the number of sliding windows
        nwindows = 9
        # Set the width of the windows +/- margin
        margin = 120
        # Set minimum number of pixels found to recenter window
        minpix = 30
    
        # Set height of windows - based on nwindows above and image shape
        window_height = np.int(binary_warped.shape[0]//nwindows)
        # Identify the x and y positions of all nonzero pixels in the image
        nonzero = binary_warped.nonzero()
        nonzeroy = np.array(nonzero[0])
        nonzerox = np.array(nonzero[1])
        # Current positions to be updated later for each window in nwindows
        leftx_current = leftx_base
        rightx_current = rightx_base
    
        # Create empty lists to receive left and right lane pixel indices
        left_lane_inds = []
        right_lane_inds = []
    
        # Step through the windows one by one
        for window in range(nwindows):
            # Identify window boundaries in x and y (and right and left)
            win_y_low = binary_warped.shape[0] - (window+1)*window_height
            win_y_high = binary_warped.shape[0] - window*window_height
            ### Find the four below boundaries of the window ###
            win_xleft_low = leftx_current - margin  # Update this
            win_xleft_high = leftx_current + margin  # Update this
            win_xright_low = rightx_current - margin  # Update this
            win_xright_high = rightx_current + margin  # Update this
            
            # Draw the windows on the visualization image
            cv2.rectangle(out_img,(win_xleft_low,win_y_low),
            (win_xleft_high,win_y_high),(0,255,0), 2) 
            cv2.rectangle(out_img,(win_xright_low,win_y_low),
            (win_xright_high,win_y_high),(0,255,0), 2) 
            
            ### Identify the nonzero pixels in x and y within the window ###
            good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & 
            (nonzerox >= win_xleft_low) &  (nonzerox < win_xleft_high)).nonzero()[0]
            good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & 
            (nonzerox >= win_xright_low) &  (nonzerox < win_xright_high)).nonzero()[0]
            
            # Append these indices to the lists
            left_lane_inds.append(good_left_inds)
            right_lane_inds.append(good_right_inds)
            
            ### If you found > minpix pixels, recenter next window ###
            ### (`right` or `leftx_current`) on their mean position ###
            if len(good_left_inds)>minpix:
                leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
            if len(good_right_inds)>minpix:
                rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
    
        # Concatenate the arrays of indices (previously was a list of lists of pixels)
        try:
            left_lane_inds = np.concatenate(left_lane_inds)
            right_lane_inds = np.concatenate(right_lane_inds)
        except ValueError:
            # Avoids an error if the above is not implemented fully
            pass
    
        # Extract left and right line pixel positions
        leftx = nonzerox[left_lane_inds]
        lefty = nonzeroy[left_lane_inds] 
        rightx = nonzerox[right_lane_inds]
        righty = nonzeroy[right_lane_inds]
    
        return leftx, lefty, rightx, righty, out_img
    
    def find_lane_pixels_sliding_window_one_side(self, binary_warped, line):
        # Split the image in half
        midpoint = np.int(binary_warped.shape[1]//2)
        
        if(line.side=='left'):
            binary_warped= binary_warped[:, :midpoint]
        else:
            binary_warped= binary_warped[:, midpoint:]
        
        # Take a histogram of the bottom half of the image
        histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0)
        # Create an output image to draw on and visualize the result
        out_img = np.dstack((binary_warped, binary_warped, binary_warped))
        # Find the peak of the left and right halves of the histogram
        # These will be the starting point for the left and right lines
        midpoint = np.int(histogram.shape[0]//2)
        x_base = np.argmax(histogram)
    
        # HYPERPARAMETERS
        # Choose the number of sliding windows
        nwindows = 9
        # Set the width of the windows +/- margin
        margin = 120
        # Set minimum number of pixels found to recenter window
        minpix = 30
    
        # Set height of windows - based on nwindows above and image shape
        window_height = np.int(binary_warped.shape[0]//nwindows)
        # Identify the x and y positions of all nonzero pixels in the image
        nonzero = binary_warped.nonzero()
        nonzeroy = np.array(nonzero[0])
        nonzerox = np.array(nonzero[1])
        # Current positions to be updated later for each window in nwindows
        x_current = x_base
    
        # Create empty lists to receive left and right lane pixel indices
        lane_inds = []
    
        # Step through the windows one by one
        for window in range(nwindows):
            # Identify window boundaries in x and y (and right and left)
            win_y_low = binary_warped.shape[0] - (window+1)*window_height
            win_y_high = binary_warped.shape[0] - window*window_height
            ### Find the four below boundaries of the window ###
            win_x_low = x_current - margin  # Update this
            win_x_high = x_current + margin  # Update this
            
            
            # Draw the windows on the visualization image
            cv2.rectangle(out_img,(win_x_low,win_y_low),
            (win_x_high,win_y_high),(0,255,0), 2) 
            
            ### Identify the nonzero pixels in x and y within the window ###
            good_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & 
            (nonzerox >= win_x_low) &  (nonzerox < win_x_high)).nonzero()[0]
            
            # Append these indices to the lists
            lane_inds.append(good_inds)
            
            ### If you found > minpix pixels, recenter next window ###
            ### (`right` or `leftx_current`) on their mean position ###
            if len(good_inds)>minpix:
                x_current = np.int(np.mean(nonzerox[good_inds]))
    
        # Concatenate the arrays of indices (previously was a list of lists of pixels)
        try:
            lane_inds = np.concatenate(lane_inds)
        except ValueError:
            # Avoids an error if the above is not implemented fully
            pass
    
        # Extract left and right line pixel positions
        if(line.side=='right'):
            x = nonzerox[lane_inds] + 640
        else:
            x = nonzerox[lane_inds]
        y = nonzeroy[lane_inds] 
    
        return x, y, out_img
    
    def search_around_poly(self, binary_warped, line):
        # Split the image in half
        midpoint = np.int(binary_warped.shape[1]//2)
        
        if(line.side=='left'):
            binary_warped = binary_warped[:, :midpoint]
        else:
            binary_warped = binary_warped[:, midpoint:]
        
        # HYPERPARAMETER
        # Choose the width of the margin around the previous polynomial to search
        # The quiz grader expects 100 here, but feel free to tune on your own!
        margin = 100
    
        # Grab activated pixels
        nonzero = binary_warped.nonzero()
        nonzeroy = np.array(nonzero[0])
        nonzerox = np.array(nonzero[1])
        
        ### TO-DO: Set the area of search based on activated x-values ###
        ### within the +/- margin of our polynomial function ###
        ### Hint: consider the window areas for the similarly named variables ###
        ### in the previous quiz, but change the windows to our new search area ###
        
        if (line.side=='right'):
            fit_x = line.best_fit[0]*(nonzeroy**2) + line.best_fit[1]*nonzeroy + line.best_fit[2] - midpoint
        else:
            fit_x = line.best_fit[0]*(nonzeroy**2) + line.best_fit[1]*nonzeroy + line.best_fit[2]
        lane_inds = ((nonzerox > (fit_x - margin)) & (nonzerox < (fit_x + margin)))
        
        
        # Again, extract left and right line pixel positions
        x = nonzerox[lane_inds]
        y = nonzeroy[lane_inds] 
    
        # Fit new polynomials
        #fitx, right_fitx, ploty = fit_poly(binary_warped.shape, leftx, lefty, rightx, righty)
        
        ## Visualization ##
        # Create an image to draw on and an image to show the selection window
        out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
        window_img = np.zeros_like(out_img)
        # Color in left and right line pixels
        out_img[nonzeroy[lane_inds], nonzerox[lane_inds]] = [255, 0, 0]
        
        ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0])
        fitx = line.best_fit[0] * ploty**2 + line.best_fit[1] * ploty + line.best_fit[2]
        if (line.side=='right'):
            fitx = fitx - midpoint
    
        # Generate a polygon to illustrate the search window area
        # And recast the x and y points into usable format for cv2.fillPoly()
        line_window1 = np.array([np.transpose(np.vstack([fitx-margin, ploty]))])
        line_window2 = np.array([np.flipud(np.transpose(np.vstack([fitx+margin, 
                                  ploty])))])
        line_pts = np.hstack((line_window1, line_window2))
    
        # Draw the lane onto the warped blank image
        cv2.fillPoly(window_img, np.int_([line_pts]), (0,255, 0))
        result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
        
        if(line.side=='right'):
            x = x + midpoint
        
        return x, y, result