# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: carnd-term1
#     language: python
#     name: myenv
# ---

# ## 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.
# * Use color transforms, gradients, etc., to create a thresholded binary image.
# * Apply a perspective transform to rectify binary image ("birds-eye view").
# * 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.
#
# ---
# ## First, I'll compute the camera calibration using chessboard images

# +
import os
import numpy as np
import cv2
import glob
from queue import Queue
import matplotlib.image as mpimg
import matplotlib.pyplot as plt
from collections import deque
# %matplotlib auto

# Define a class to receive the characteristics of each line detection
class Line():
    def __init__(self,buffer_size):
        self.buffer_size = buffer_size
        # was the line detected in the last iteration?
        self.detected = False  
        # x values of the last n fits of the line
        self.recent_xfitted = deque([],maxlen=self.buffer_size)         # format [x1,x2...x5]
        #average x values of the fitted line over the last n iterations
        self.bestx = deque([],maxlen=self.buffer_size)     
        #polynomial coefficients averaged over the last n iterations
        # x = A * y ** 2 + B * y + C
        self.best_fit = {'A':deque([],maxlen=self.buffer_size),'B':deque([],maxlen=self.buffer_size),
                      'C':deque([],maxlen=self.buffer_size)}  
        self.best_fitm = {'A':deque([],maxlen=self.buffer_size),'B':deque([],maxlen=self.buffer_size),
                      'C':deque([],maxlen=self.buffer_size)}      # by unit meters
        #polynomial coefficients for the most recent fit, i.e. current frame
        self.current_fit = np.array([0,0,0], dtype='float')
        self.current_fitm = np.array([0,0,0], dtype='float')      # by unit meters
        #radius of curvature of the line in some units
        self.radius_of_curvature = deque([],maxlen=self.buffer_size) 
        #distance in meters of vehicle center from the line
        self.line_base_pos = deque([],maxlen=self.buffer_size) 
        #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 = deque([],maxlen=self.buffer_size)  
        #y values for detected line pixels
        self.ally = deque([],maxlen=self.buffer_size)
        
class Lane():
    def __init__(self):
        self.ym_per_pix = None           # y dimension, transfer pix scale to real world meter scale
        self.xm_per_pix = None           # x dimension, transfer pix scale to real world meter scale
        self.mtx = None                  # camera calibration mtx
        self.dist = None                 # camera calibration dst
        self.count = 0

left_line = Line(5)
right_line = Line(5)
lane = Lane()

# Make a list of calibration images
imgfiles = glob.glob('camera_cal/calibration*.jpg')

def camera_calibration(imgfiles,display = False):
    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    # x axis 9 pionts, y axis 6 points, scan from x axis, one piont by one piont
    objp = np.zeros((6*9,3), np.float32)
    objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)  
    
    # Arrays to store object points and image points from all the images.
    objpoints = [] # 3d points in real world space
    imgpoints = [] # 2d points in image plane.

    # Step through the list and search for chessboard corners
    for fname in imgfiles:
        img = cv2.imread(fname)
        gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

        # Find the chessboard corners
        ret, corners = cv2.findChessboardCorners(gray, (9,6),None)  # corners are 9 x 6 = 54 coordinates
        # If found, add object points, image points
        if ret == True:
            objpoints.append(objp)
            imgpoints.append(corners)

    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, 
                                                gray.shape[::-1], None, None)
    
    return mtx, dist

mtx, dist = camera_calibration(imgfiles)
lane.mtx = mtx
lane.dist = dist


# -

# ## Apply a distortion correction to raw images.

# +
# %matplotlib auto
# undistort image with camera calibration mtx, dist
def undistort(img,mtx, dist):
    undist = cv2.undistort(img, mtx, dist, None, mtx)
    return undist

img = mpimg.imread('camera_cal/calibration1.jpg')
undist = undistort(img, mtx, dist)

f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax1.set_title('Original Image')
ax2.imshow(undist)
ax2.set_title('Undistorted Image')
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)


# -

# ## Use color transforms, gradients, etc., to create a thresholded binary image.

# +
# define color and x-gradient filter
# b channel filter yellow
# l channel filter white
# l_thresh=(200, 255), b_thresh=(155,200)
def image_filter(img, l_thresh=(200, 255), b_thresh=(155,200),sx_thresh=(70, 210)):
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    # convert to LUV color space
    luv = cv2.cvtColor(img,cv2.COLOR_RGB2Luv)
    luv_channel = luv[:,:,0]
    u_channel = luv[:,:,1]
    v_channel = luv[:,:,2]
    luv_binary = np.zeros_like(luv_channel)
    # luv_threshold = (230,255)
    u_threshold = (0,255)
    v_threshold = (0,255)
    l_th = (luv_channel >= l_thresh[0]) & (luv_channel <=l_thresh[1])
    u_th = (u_channel >= u_threshold[0]) & (u_channel <=u_threshold[1])
    v_th = (v_channel >= v_threshold[0]) & (v_channel <=v_threshold[1])
    luv_binary[l_th & u_th & v_th] =1
    # convert to Lab color space
    lab = cv2.cvtColor(img,cv2.COLOR_RGB2Lab)
    lab_channel = lab[:,:,0]
    a_channel = lab[:,:,1]
    b_channel = lab[:,:,2]
    lab_binary = np.zeros_like(b_channel)
    lab_threshold = (0,255)
    a_threshold = (0,255)
    # b_threshold = (170,255) 
    lab_th = (lab_channel >= lab_threshold[0]) & (lab_channel <=lab_threshold[1])
    a_th = (a_channel >= a_threshold[0]) & (a_channel <=a_threshold[1])
    b_th = (b_channel >= b_thresh[0]) & (b_channel <=b_thresh[1])
    lab_binary[lab_th & a_th & b_th] = 1
    
    # Sobel x
    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0) # Take the derivative in x
    abs_sobelx = np.absolute(sobelx) # Absolute x derivative to accentuate lines away from horizontal
    scaled_sobel = np.uint8(255*abs_sobelx/np.max(abs_sobelx))
    
    # Threshold x gradient
    sxbinary = np.zeros_like(scaled_sobel)
    sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])] = 1
    
    # combined channel
    combined = np.zeros_like(sxbinary)
    combined[ ((luv_binary == 1) | (lab_binary ==1) | (sxbinary == 1)) ] = 1
    return combined

# undist = mpimg.imread('output_images/straight_lines1.jpg')
undist = mpimg.imread('./output_images/straight_lines1.jpg')
combined = image_filter(undist)
# cv2.imwrite('output_images/binary_combined.jpg',filtered)
# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()

ax1.imshow(undist)
ax1.set_title('Undistorted Image', fontsize=40)

ax2.imshow(combined,cmap='gray')
# plt.savefig('output_images/binary_combined.jpg')
ax2.set_title('Binary threshold Image', fontsize=40)
plt.subplots_adjust(left=0.02, right=1, top=0.9, bottom=0.)


# -

# ## Apply ROI to binary image

# +
def region_mask(img):
    mask = np.zeros_like(img)
    # inner_mask = np.zeros_like(img)
    h,w = mask.shape[0],mask.shape[1]

    vertices = np.array([[(0.4*w-50,h*0.7),(0.6*w+50,h*0.7),(w*0.85+100, h),
                                (w*0.15-100,h)]],dtype=np.int32)
    
    # '''    
    # inner_vertices = np.array([[(0.45*w+20,h*0.7),(0.55*w-20,h*0.7),(w*0.8-200, h),
    #                             (w*0.2+200,h)]],dtype=np.int32)
    # '''
    cv2.fillPoly(mask,vertices,1)
    # cv2.fillPoly(mask,inner_vertices,0)
    masked = cv2.bitwise_and(img,mask)
    return masked

masked = region_mask(combined)
f = plt.figure(figsize=(24,9))
plt.imshow(masked,cmap='gray')
# -

# ## Apply a perspective transform to rectify binary image ("birds-eye view").

# +
# test = mpimg.imread('output_images/straight_lines1.jpg')
test = mpimg.imread('output_images/straight_lines1.jpg')

imshape = masked.shape
h = imshape[0]
w = imshape[1]

# select 4 source points
src = [[w*0.4+5,h*0.7],[w*0.6,h*0.7],[w*0.85+10, h],[w*0.15+10, h]]
# src = [[w*0.45-5,h*0.65],[w*0.55+10,h*0.65],[w*0.77+5, h*0.9],[w*0.23+15, h*0.9]]
# lane.src = src
# select 4 destination points
dst = [[w*0.15,0],[w*0.85,0],[w*0.85, h],[w*0.15, h]]
# dst = [[w*0.23,0],[w*0.77,0],[w*0.77+5, h],[w*0.23+5, h]]
# lane.dst = dst


# perspective transform
def perspective_transform(img,src,dst):
    src_pts = np.float32(src)
    dst_pts = np.float32(dst)
    # use src, dst points to compute M
    M = cv2.getPerspectiveTransform(src_pts, dst_pts)
    # Warp an image using the perspective transform, M
    warped = cv2.warpPerspective(img, M, (w,h), flags=cv2.INTER_LINEAR)
    return warped

def draw_lines(img,points):
    pts = np.array(points, np.int32)
    pts = pts.reshape((-1,1,2))
    # print(pts)
    cv2.polylines(img,[pts],True,(255,0,0),5)
    return img

# src, dst = get_src_dst(masked)
test_warped = perspective_transform(test,src,dst)

# draw source and destination points to tune points parameter,when setup completed, comment the draw function.
# draw source points on undistorted image
draw_lines(test,src)
# draw destination points on warped image
draw_lines(test_warped,dst)

# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(test)
ax1.set_title('Undistorted Image', fontsize=40)

ax2.imshow(test_warped)
ax2.set_title('Warped Image', fontsize=40)
plt.subplots_adjust(left=0.02, right=1.0, top=0.9, bottom=0.)


# -

# ## Detect lane pixels and fit to find the lane boundary.

# +
def search_window(img,out_img,minpix,x_center,y_bottom,margin,height,display):
    
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    
    win_y_low = y_bottom
    win_y_high = y_bottom + height
    win_x_low = x_center - margin  # Update this
    win_x_high = x_center + margin # Update this
    
    ### 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] 
    
    line_found = False
    line_center = x_center
    line_inds = []
    if len(good_inds) > minpix:
        line_found = True
        line_center = np.int(np.mean(nonzerox[good_inds]))
        line_inds = good_inds
        if display:
            # 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) 
    
    return line_found,line_center,line_inds

def find_lane_pixels(binary_warped,display=False):
    # 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])
    # print('leftx_base: ',leftx_base)
    rightx_base = np.argmax(histogram[midpoint:]) + midpoint 
    # print('rightx_base: ',rightx_base)

    # HYPERPARAMETERS
    # Choose the number of sliding windows
    nwindows = 9
    # Set the width of the windows +/- margin
    margin = 100
    # Set minimum number of pixels found to recenter window
    minpix = 50
    # Set shift of the windows when the curve lane run out of window margin
    shift = round(1.5 * margin)
    
    nonzero = binary_warped.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])

    # Set height of windows - based on nwindows above and image shape
    window_height = np.int(binary_warped.shape[0]//nwindows)
    
    # Current positions to be updated later for each window in nwindows
    leftx_current = leftx_base
    rightx_current = rightx_base
    
    lane.centroid = (leftx_base+rightx_base)/2
    lane.ym_per_pix = 30/binary_warped.shape[0]      # y dimension, transfer pix scale to real world meter scale
    lane.xm_per_pix = 3.7/(rightx_base-leftx_base)      # x dimension, transfer pix scale to real world meter scale
    left_line.line_base_pos = lane.xm_per_pix * (midpoint - leftx_base)
    right_line.line_base_pos = lane.xm_per_pix * (midpoint - 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):
        y_bottom = binary_warped.shape[0] - (window+1)*window_height
        leftx_center = leftx_current
        left_line_found,leftx_current,left_line_inds = search_window(binary_warped,out_img,minpix,
                                                                     leftx_center,y_bottom,
                                                                     margin,window_height,display)
        if not left_line_found:
            leftx_center = leftx_current - shift
            left_line_found,leftx_current,left_line_inds = search_window(binary_warped,out_img,minpix,
                                                                         leftx_center,y_bottom,
                                                                         margin,window_height,display)
            if not left_line_found:
                leftx_center = leftx_current + shift
                left_line_found,leftx_current,left_line_inds = search_window(binary_warped,out_img,minpix,
                                                                             leftx_center,y_bottom,
                                                                             margin,window_height,display)
        rightx_center = rightx_current
        right_line_found,rightx_current,right_line_inds = search_window(binary_warped,out_img,minpix,
                                                                        rightx_center,y_bottom,
                                                                        margin,window_height,display)
        if not right_line_found:
            rightx_center = rightx_current - shift
            right_line_found,rightx_current,right_line_inds = search_window(binary_warped,out_img,minpix,
                                                                            rightx_center,y_bottom,
                                                                            margin,window_height,display)
            if not right_line_found:
                rightx_center = rightx_current + shift
                right_line_found,rightx_current,right_line_inds = search_window(binary_warped,out_img,minpix,
                                                                                rightx_center,y_bottom,
                                                                                margin,window_height,display)
        if len(left_line_inds):
            left_lane_inds.append(left_line_inds)
        if len(right_line_inds):
            right_lane_inds.append(right_line_inds)
    
    # Get left and right line pixel positions
    if len(left_lane_inds):
        left_lane_inds = np.concatenate(left_lane_inds)
        leftx = nonzerox[left_lane_inds]
        lefty = nonzeroy[left_lane_inds] 
    else:
        leftx = left_line.allx[-1]
        # print('leftx: \n',leftx)
        lefty = left_line.ally[-1]
        # print('lefty: \n',lefty)
    if len(right_lane_inds):
        right_lane_inds = np.concatenate(right_lane_inds)
        rightx = nonzerox[right_lane_inds]
        righty = nonzeroy[right_lane_inds]
    else:
        rightx = right_line.allx[-1]
        # print('rightx: \n',rightx)
        righty = right_line.ally[-1]
        # print('righty: \n',righty)

    return leftx, lefty, rightx, righty, out_img

def sliding_window(binary_warped,display = False):
    
    # Find our lane pixels first
    leftx, lefty, rightx, righty, out_img = find_lane_pixels(binary_warped,display)
    
    # transfer x,y position from pix scale to real world
    # leftxm = lane.xm_per_pix * leftx
    # leftym = lane.ym_per_pix * lefty
    # rightxm = lane.xm_per_pix * rightx
    # rightym = lane.ym_per_pix * righty

    ### TO-DO: Fit a second order polynomial to each using `np.polyfit` ###
    
    left_fit = np.polyfit(lefty,leftx, 2)
    # left_fitm = np.polyfit(leftym,leftxm, 2)
    right_fit = np.polyfit(righty,rightx,2)
    # right_fitm = np.polyfit(rightym,rightxm, 2)

    # Generate x and y values for plotting
    ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0])
    try:
        left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
        right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
    except TypeError:
        # Avoids an error if `left` and `right_fit` are still none or incorrect
        print('The function failed to fit a line!')
        left_fitx = 1*ploty**2 + 1*ploty
        right_fitx = 1*ploty**2 + 1*ploty
    
    # left_line.detected = True
    # right_line.detected = True
    left_line.current_fit = left_fit
    # left_line.current_fitm = left_fitm
    right_line.current_fit = right_fit
    # right_line.current_fitm = right_fitm
    left_line.recent_xfitted.append(left_fitx)
    right_line.recent_xfitted.append(right_fitx)
    
    ## Visualization ##
    # Colors in the left and right lane regions
    if display:
        out_img[lefty, leftx] = [255, 0, 0]
        out_img[righty, rightx] = [0, 0, 255]
    
        pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
        pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
        pts = np.hstack((pts_left, pts_right))
        cv2.polylines(out_img,np.int_([pts]),True,(255,255,0),2)
        plt.imshow(out_img)
    
    return leftx, lefty, rightx, righty

binary_warped = perspective_transform(masked,src,dst)
leftx, lefty, rightx, righty = sliding_window(binary_warped,True)

# -

# ## Determine the curvature of the lane and vehicle position with respect to center.

# +
def measure_curvature_meters(ym,line_fitm):
    '''
    Calculates the curvature of polynomial functions in pixels.
    '''
    ##### TO-DO: Implement the calculation of R_curve (radius of curvature) #####
    curverad = ((1+(2*line_fitm[0]*ym + line_fitm[1])**2)**(3/2))/np.abs(2*line_fitm[0])
    
    return curverad


ym = lane.ym_per_pix * (binary_warped.shape[0]-1)
left_fitm = np.polyfit(lane.ym_per_pix * lefty, lane.xm_per_pix *leftx,2)
right_fitm = np.polyfit(lane.ym_per_pix * righty, lane.xm_per_pix *rightx,2)

left_curvature = measure_curvature_meters(ym,left_fitm)
right_curvature = measure_curvature_meters(ym,right_fitm)
average_curverad = (left_curvature + right_curvature)/2
offset = (left_line.line_base_pos + right_line.line_base_pos)/2
print('average_curverad: ',average_curverad)
print('vehicle offset: ', offset)

# left_curverad, right_curverad = measure_curvature_pixels(ploty,left_fit, right_fit)
# print(left_curverad, right_curverad)
# -

# ## Warp the detected lane boundaries back onto the original image.

# +
# Create an image to draw the lines on
def draw_back(org_img,binary_img,src,dst,ploty,leftx,rightx):
    warp_zero = np.zeros_like(binary_img).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
    
    # Recast the x and y points into usable format for cv2.fillPoly()
    pts_left = np.array([np.transpose(np.vstack([leftx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([rightx, ploty])))])
    pts = np.hstack((pts_left, pts_right))

    # Draw the lane onto the warped blank image
    cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))

    src_pts = np.float32(src)
    dst_pts = np.float32(dst)
    
    # compute inverse M transformation martix
    Minv = cv2.getPerspectiveTransform(dst_pts, src_pts)
    # Warp the blank back to original image space using inverse perspective matrix (Minv)
    newwarp = cv2.warpPerspective(color_warp, Minv, (img.shape[1], img.shape[0])) 
    # Combine the result with the original image
    draw_back_img = cv2.addWeighted(org_img, 1, newwarp, 0.4, 0)
    return draw_back_img

ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0])
left_fitx = left_line.recent_xfitted[-1]
right_fitx= right_line.recent_xfitted[-1]
result = draw_back(undist,binary_warped,src,dst,ploty,left_fitx,right_fitx)
# plot the result
f = plt.figure(figsize=(24,9))
plt.imshow(result)


# -

# ## search around previously polynomial fit

# +
def fit_poly(img_shape, leftx, lefty, rightx, righty):
    if len(leftx) & len(rightx):
        # transfer x,y position from pix scale to real world
        # leftxm = lane.xm_per_pix * leftx
        # leftym = lane.ym_per_pix * lefty
        # rightxm = lane.xm_per_pix * rightx
        # rightym = lane.ym_per_pix * righty
        
        ### TO-DO: Fit a second order polynomial to each with np.polyfit() ###
        left_fit = np.polyfit(lefty,leftx, 2)
        # left_fitm = np.polyfit(leftym,leftxm, 2)
        right_fit = np.polyfit(righty,rightx,2)
        # right_fitm = np.polyfit(rightym,rightxm, 2)
        
        left_line.current_fit = left_fit
        # left_line.current_fitm = left_fitm
        right_line.current_fit = right_fit
        # right_line.current_fitm = right_fitm
        
    else:
        left_fit = left_line.current_fit
        right_fit = right_line.current_fit
        
    # Generate x and y values for plotting
    ploty = np.linspace(0, img_shape[0]-1, img_shape[0])
    ### TO-DO: Calc both polynomials using ploty, left_fit and right_fit ###
    left_fitx = left_fit[0]*(ploty**2) + left_fit[1]*ploty + left_fit[2]
    right_fitx = right_fit[0]*(ploty**2) + right_fit[1]*ploty + right_fit[2]
    
    left_line.recent_xfitted.append(left_fitx)
    right_line.recent_xfitted.append(right_fitx)
    
    return left_fitx, right_fitx, ploty

def search_around_poly(binary_warped,display = False):
    # HYPERPARAMETER
    # Choose the width of the margin around the previous polynomial to search
    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 ###
    left_fit = left_line.current_fit
    right_fit = right_line.current_fit
    left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + 
                    left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) + 
                    left_fit[1]*nonzeroy + left_fit[2] + margin)))
    right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + 
                    right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) + 
                    right_fit[1]*nonzeroy + right_fit[2] + margin)))
    
    #print('left lane points: ',len(left_lane_inds))
    #print('right lane points: ',len(right_lane_inds))
    
    # Again, 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]

    # Fit new polynomials
    left_fitx,right_fitx,ploty = fit_poly(binary_warped.shape, leftx, lefty, rightx, righty)
    
    # 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)
    ## Visualization ##
    if display:
        
        # Color in left and right line pixels
        out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
        out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]

        # Generate a polygon to illustrate the search window area
        # And recast the x and y points into usable format for cv2.fillPoly()
        left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
        left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, 
                              ploty])))])
        left_line_pts = np.hstack((left_line_window1, left_line_window2))
        right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
        right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, 
                              ploty])))])
        right_line_pts = np.hstack((right_line_window1, right_line_window2))

        # Draw the lane onto the warped blank image
        cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
        cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
        out_img = cv2.addWeighted(out_img, 1, window_img, 0.4, 0)
    
        # Plot the polynomial lines onto the image
        pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
        pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
        pts = np.hstack((pts_left, pts_right))
        cv2.polylines(out_img,np.int_([pts]),True,(255,255,0),2)
        plt.imshow(out_img)
        ## End visualization steps ##
    
    return leftx,lefty,rightx,righty

# Run image through the pipeline
# Note that in your project, you'll also want to feed in the previous fits
leftx,lefty,rightx,righty = search_around_poly(binary_warped,True)


# +
def is_outlier(best_fit,line_pos):
    # at the begining, best_fit array length < 5, outlier set to False.
    outlier = False
    # only when best_fit deque full, length == 5, check x position validation.
    if len(best_fit) ==5:
        p = np.min(best_fit)-50
        q = np.max(best_fit) + 50
        if (line_pos <p) & (line_pos > q):
            outlier = True
        
    return outlier
        
def check_lane(left_fit,right_fit,h,w):
    left_line_valid = True
    right_line_valid = True
    # get left line top x position, top y position is 0
    left_line_top = left_fit[2]
    outlier = is_outlier(left_line.best_fit['C'],left_line_top)
    if outlier:
        left_line_valid = False
    # get left line bottom x position, bottom y position is h
    left_line_bottom = left_fit[0] * h ** 2 + left_fit[1] * h + left_fit[2]
    # get left line best_fit x position, bottom y position is h
    left_best_bottom =np.array(left_line.best_fit['A'])*h**2+np.array(left_line.best_fit['B'])*h \
                               +np.array(left_line.best_fit['C'])
    outlier = is_outlier(left_best_bottom,left_line_bottom)
    if outlier:
        left_line_valid = False
        
    # get right line top x position, top y position is 0
    right_line_top = right_fit[2]
    outlier = is_outlier(right_line.best_fit['C'],right_line_top)
    if outlier:
        right_line_valid = False
    # get right line bottom x position, bottom y position is h
    right_line_bottom = np.array(right_fit[0] ** h + right_fit[1] * h + right_fit[2])
    # get right line best_fit x position, bottom y position is h
    right_best_bottom =np.array(right_line.best_fit['A'])*h**2+np.array(right_line.best_fit['B'])*h \
                                +np.array(right_line.best_fit['C'])
    outlier = is_outlier(right_best_bottom,right_line_bottom)
    if outlier:
        right_line_valid = False
    
    # x = A*y**2 + B*y + C
    # x_top = C
    # x_bottom = A*h**2 + B*h + C
    # slope = (x_bottom - x_top)/h = A*h + B
    left_right_match = True
    # absolute value of diffrence between left_line slope and right_line slope <= w/h
    if np.abs((left_fit[0]*h + left_fit[1]) - (right_fit[0]+right_fit[1])) > w/h:
        left_right_match = False
    
    # the distance between right_bottom and left_bottom must >= w/2
    if (right_fit[0]*h**2 + right_fit[1]*h + right_fit[2])-(left_fit[0]*h**2 + left_fit[1]*h + left_fit[2])<w/2:
        left_right_match = False
    
    # the distance between right_top and left_top must >= w/2
    if (right_fit[2] - left_fit[2]) <w/3:
        
        left_right_match = False
        
    if left_line_valid & right_line_valid:
        if left_right_match == True:
            return True, 
    
def update_line(line,item,line_fit):
    if item == 'best_fit':
        if line == 'left_line':
            left_line.best_fit['A'].append(line_fit[0])
            left_line.best_fit['B'].append(line_fit[1])
            left_line.best_fit['C'].append(line_fit[2])
        if line == 'right_line':
            right_line.best_fit['A'].append(line_fit[0])
            right_line.best_fit['B'].append(line_fit[1])
            right_line.best_fit['C'].append(line_fit[2])
    elif item == 'best_fitm':
        if line == 'left_line':
            left_line.best_fitm['A'].append(line_fit[0])
            left_line.best_fitm['B'].append(line_fit[1])
            left_line.best_fitm['C'].append(line_fit[2])
        if line == 'right_line':
            right_line.best_fitm['A'].append(line_fit[0])
            right_line.best_fitm['B'].append(line_fit[1])
            right_line.best_fitm['C'].append(line_fit[2])
            
    
def multiframe_smooth(line,item):
    if item == 'best_fit':
        if line == 'left_line':
            left_fit_smooth = {'A':0,'B':0,'C':0}
            left_fit_smooth['A'] = np.mean(left_line.best_fit['A'])
            left_fit_smooth['B'] = np.mean(left_line.best_fit['B'])
            left_fit_smooth['C'] = np.mean(left_line.best_fit['C'])
            return left_fit_smooth
        if line == 'right_line':
            right_fit_smooth = {'A':0,'B':0,'C':0}
            right_fit_smooth['A'] = np.mean(right_line.best_fit['A'])
            right_fit_smooth['B'] = np.mean(right_line.best_fit['B'])
            right_fit_smooth['C'] = np.mean(right_line.best_fit['C'])
            return right_fit_smooth
        
    if item == 'curvature':
        if line == 'left_line':
            left_curvature_smooth = np.mean(left_line.radius_of_curvature)
            return left_curvature_smooth
        if line == 'right_line':
            right_curvature_smooth = np.mean(right_line.radius_of_curvature)
            return right_curvature_smooth

def generate_line(ploty,line_fit):
        '''
        input ploty values
        generate lane line based on polyfit cofficient
        polyfit coeffs:{'A':,'B':,'C':}
        returns array of [y,x] points ,y along image vertical / x along image horizontal direction
        '''
        # y_pts = np.linspace(0,height-1,height)
        x_pts = line_fit['A']*ploty**2+line_fit['B']*ploty+line_fit['C']
        return x_pts


# +
left_line = Line(5)
right_line = Line(5)
lane = Lane()

def pipeline(img):
    h,w = img.shape[0],img.shape[1]
    undist = undistort(img,mtx, dist)
    combined = image_filter(undist)
    masked = region_mask(combined)    
    binary_warped = perspective_transform(masked,src,dst)
    # first run slide window, or x,y pixels not found in previous frame, then run slide window
    if (left_line.detected == False) | (right_line.detected == False): 
        leftx, lefty, rightx, righty = sliding_window(binary_warped)
        left_line.detected = True
        right_line.detected = True
    # if x,y pixels found in previous frame, search around previous left_fit, right_fit
    else:
        leftx, lefty, rightx, righty = search_around_poly(binary_warped) 
    # if leftx and rightx found in current frame,append to allx and ally deque    
    if len(leftx) & len(rightx):
        left_line.allx.append(leftx)
        left_line.ally.append(lefty)
        right_line.allx.append(rightx)
        right_line.ally.append(righty)
    else:
        left_line.detected = False
        right_line.detected = False
    
    multiframe_left = {'x':[val for sublist in left_line.allx for val in sublist],
                   'y':[val for sublist in left_line.ally for val in sublist]}
    multiframe_right = {'x':[val for sublist in right_line.allx for val in sublist],
                   'y':[val for sublist in right_line.ally for val in sublist]}
    # print('multiframe_left: \n',multiframe_left)
    left_fit = np.polyfit(multiframe_left['y'],multiframe_left['x'],2)
    right_fit = np.polyfit(multiframe_right['y'],multiframe_right['x'],2)
    left_fit_real = np.polyfit([i*lane.ym_per_pix for i in multiframe_left['y']],
                                          [i*lane.xm_per_pix for i in multiframe_left['x']],2)
    right_fit_real = np.polyfit([i*lane.ym_per_pix for i in multiframe_right['y']],
                                          [i*lane.xm_per_pix for i in multiframe_right['x']],2)
    
    if check_lane(left_fit,right_fit,h,w):
        # left_line.detected = True
        # right_line.detected = True
        update_line('left_line','best_fit',left_fit)
        update_line('right_line','best_fit',right_fit)
        update_line('left_line','best_fitm',left_fit_real)
        update_line('right_line','best_fitm',right_fit_real)
        
        ym = lane.ym_per_pix * (binary_warped.shape[0]-1)
        left_curvature = measure_curvature_meters(ym, left_fit_real)
        right_curvature = measure_curvature_meters(ym, right_fit_real)
        left_line.radius_of_curvature.append(left_curvature)
        right_line.radius_of_curvature.append(right_curvature)
    else:
        left_line.detected = False
        right_line.detected = False
        left_fit = {'A':left_line.best_fit['A'][-1],'B':left_line.best_fit['B'][-1],
                    'C':left_line.best_fit['C'][-1]}
        right_fit = {'A':right_line.best_fit['A'][-1],'B':right_line.best_fit['B'][-1],
                     'C':right_line.best_fit['C'][-1]}
        left_curvature = left_line.radius_of_curvature[-1]
        right_curvature = right_line.radius_of_curvature[-1]
        
    # smooth multiframe(5 frame best_fit)
    left_fit_smooth = multiframe_smooth('left_line','best_fit')
    right_fit_smooth = multiframe_smooth('right_line','best_fit')
    left_curvature_smooth = multiframe_smooth('left_line','curvature')
    right_curvature_smooth = multiframe_smooth('right_line','curvature')
    
    # generate left_fitx, right_fitx based on 5 frame best_fit coef
    ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0])
    left_fitx = generate_line(ploty,left_fit_smooth)
    right_fitx = generate_line(ploty,right_fit_smooth)
    center_of_lane = (left_fitx[-1] + right_fitx[-1])/2
    offset = (binary_warped.shape[1]/2 - center_of_lane)*lane.xm_per_pix
    average_curverad = (left_curvature_smooth + right_curvature_smooth)/2
                                                   
    result = draw_back(undist,binary_warped,src,dst,ploty,left_fitx,right_fitx)
    
    shift = 'right '
    if offset <0:
        shift = 'left '
        offset = -offset
        
    cv2.putText(result,'Radius of Curvature: '+str(np.round(average_curverad)).rjust(6)+'(m)',(50,50),
                cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),thickness=2)
    cv2.putText(result,'Vehicle is ' + str(np.round(offset,2))+'m '+shift.rjust(5)+'of center',
            (50,100),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),thickness=2)
    
    return result


# +
# Import everything needed to edit/save/watch video clips
from moviepy.editor import VideoFileClip
# from moviepy.editor import *
from IPython.display import HTML

project_video_output = 'output_videos/project_video.mp4'
## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video
## To do so add .subclip(start_second,end_second) to the end of the line below
## Where start_second and end_second are integer values representing the start and end of the subclip
## You may also uncomment the following line for a subclip of the first 5 seconds
## clip1 = VideoFileClip("test_videos/solidWhiteRight.mp4").subclip(0,5)

clip1 = VideoFileClip("project_video.mp4")
output_clip = clip1.fl_image(pipeline) #NOTE: this function expects color images!!
# %time output_clip.write_videofile(project_video_output, audio=False)
# -

HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(project_video_output))

# ### try challenge_video

# +
from moviepy.editor import VideoFileClip
# from moviepy.editor import *
from IPython.display import HTML

challenge_video_output = 'output_videos/challenge_video.mp4'
## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video
## To do so add .subclip(start_second,end_second) to the end of the line below
## Where start_second and end_second are integer values representing the start and end of the subclip
## You may also uncomment the following line for a subclip of the first 5 seconds
##clip1 = VideoFileClip("test_videos/solidWhiteRight.mp4").subclip(0,5)

clip1 = VideoFileClip("challenge_video.mp4")
output_clip = clip1.fl_image(pipeline) #NOTE: this function expects color images!!
# %time output_clip.write_videofile(challenge_video_output, audio=False)
# -

HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(challenge_video_output))

# ## try harder_challenger_video

# +
from moviepy.editor import VideoFileClip
# from moviepy.editor import *
from IPython.display import HTML

harder_challenge_video_output = 'output_videos/harder_challenge_video.mp4'
## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video
## To do so add .subclip(start_second,end_second) to the end of the line below
## Where start_second and end_second are integer values representing the start and end of the subclip
## You may also uncomment the following line for a subclip of the first 5 seconds
##clip1 = VideoFileClip("test_videos/solidWhiteRight.mp4").subclip(0,5)

clip1 = VideoFileClip("harder_challenge_video.mp4")
output_clip = clip1.fl_image(pipeline) #NOTE: this function expects color images!!
# %time output_clip.write_videofile(harder_challenge_video_output, audio=False)
# -

HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(harder_challenge_video_output))

StarcoderdataJupyterScriptsDedupFiltered