In [1]:

    

# Activate/Deactivate steps visualisation
vis = 1
# Activate/Deactivate Test video
test_video = 0
# Activate/Deactivate Project video
proj_video = 1
# Activate/Deactivate Challenge video
chal_video = 1
# Activate/Deactivate HarderChallenge video
hardchal_video = 0


    

In [2]:

    

import pickle

calibration_file = 'wide_dist_pickle.p'

with open(calibration_file, mode='rb') as f:
    calib = pickle.load(f)

mtx = calib['mtx']
dist = calib['dist']


    

In [3]:

    

import matplotlib.pyplot as plt
import matplotlib.image as mpimg
%matplotlib inline

if vis == 1:
    my_dpi = 94
    aspect_ratio = 16/9

    x_pix = 600  # x dimension of resulting figure 
    col_num = 1
    row_num = 2

    img_straight = {}
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    for i in range(2):
        img_straight[i] = mpimg.imread('test_images/straight_lines' + str(i+1) + '.jpg')
        ax[i].imshow(img_straight[i])
        ax[i].set_title('Straight Image ' + str(i+1))
        ax[i].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/straight_images.png", dpi=my_dpi)


    

In [4]:

    

import cv2

if vis == 1:
    x_pix = 1200  # x dimension of resulting figure 
    col_num = 2
    row_num = 2

    undist_img_straight = {}
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    for i in range(2):
        undist_img_straight[i] = cv2.undistort(img_straight[i], mtx, dist, None, mtx)
        ax[i, 0].imshow(img_straight[i])
        ax[i, 0].set_title('Straight Lane Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')
        ax[i, 1].imshow(undist_img_straight[i])
        ax[i, 1].set_title('Undistorded Straight Lane Image' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/undistorded_image_straight.png", dpi=my_dpi)


    

In [5]:

    

import numpy as np

h = 720
w = 1280 

# Source
y1  = 475
x11 = 560
x12 = 727

y2  = 720
x21 = 216
x22 = 1115

pt1 = np.float32([[x11,y1], [x12,y1], 
                  [x22,y2], [x21,y2]])

# Destination
y1  = 0
x11 = 440
x12 = 840

y2  = 720
x21 = 440
x22 = 840

pt2 = np.float32([[x11,y1], [x12,y1], 
                  [x22,y2], [x21,y2]])

# Transform direct and inverse matrices
M = cv2.getPerspectiveTransform(pt1, pt2)
Minv = cv2.getPerspectiveTransform(pt2, pt1)

if vis == 1:
    # Results visualization

    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 2
    row_num = 2

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    warped_straight = {}

    for i in range(2):
        warped_straight[i] = cv2.warpPerspective(undist_img_straight[i], M, (w, h), flags=cv2.INTER_LINEAR)

        ax[i, 0].imshow(undist_img_straight[i])
        for j in range(4):
            ax[i, 0].plot(pt1[j][0], pt1[j][1], 'rs')
        ax[i, 0].set_title('Straight Image ' + str(i+1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(warped_straight[i])
        for j in range(4):
            ax[i, 1].plot(pt2[j][0], pt2[j][1], 'rs')
        ax[i, 1].set_title('Bird-eye View Image ' + str(i+1), fontsize=15)
        ax[i, 1].axis('off')

    f.tight_layout()

    plt.savefig("./output_images/perspective_xform_straight_images.png", dpi=my_dpi)


    

In [6]:

    

if vis == 1:
    x_pix = 400  # x dimension of resulting figure 
    col_num = 1
    row_num = 6

    img = {}
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    for i in range(6):
        img[i] = mpimg.imread('test_images/test' + str(i+1) + '.jpg')
        ax[i].imshow(img[i])
        ax[i].set_title('Test Image ' + str(i+1))
        ax[i].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/test_images.png", dpi=my_dpi)


    

In [7]:

    

if vis == 1:
    x_pix = 800  # x dimension of resulting figure 
    col_num = 2
    row_num = 6

    undist_img = {}
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    for i in range(6):
        undist_img[i] = cv2.undistort(img[i], mtx, dist, None, mtx)
        ax[i, 0].imshow(img[i])
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')
        ax[i, 1].imshow(undist_img[i])
        ax[i, 1].set_title('Undistorded Image ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/undistorded_image.png", dpi=my_dpi)


    

In [8]:

    

if vis == 1:
    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 2
    row_num = 6

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    warped = {}

    for i in range(6):
        warped[i] = cv2.warpPerspective(undist_img[i], M, (w, h), flags=cv2.INTER_LINEAR)

        ax[i, 0].imshow(undist_img[i])
        ax[i, 0].set_title('Test Image ' + str(i+1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(warped[i])
        ax[i, 1].set_title('Bird-eye View Image ' + str(i+1), fontsize=15)
        ax[i, 1].axis('off')

    f.tight_layout()

    plt.savefig("./output_images/perspective_xform_test_images.png", dpi=my_dpi)


    

In [9]:

    

def gaussian_blur(img, kernel_size=5):
    """Applies a Gaussian Noise kernel"""
    return cv2.GaussianBlur(img, (kernel_size, kernel_size), 0)


    

In [10]:

    

def CLAHE_contrast_normalization(image, clipLimit=2.0, tileGridSize=(8,8)):
    
    # create a CLAHE object
    clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8,8))
    temp = clahe.apply(image)

    return temp


    

In [11]:

    

import numpy as np

if vis == 1:
    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 3
    row_num = 6

    # Plot the result
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))
    hls = {}
    rgb_enhanced = {}

    for i in range(6):
        lab = cv2.cvtColor(warped[i], cv2.COLOR_RGB2LAB)
        l_chan = lab[:, :, 0]
        a_chan = lab[:, :, 1]
        b_chan = lab[:, :, 2]

        ax[i, 0].imshow(l_chan, cmap='gray')
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        enhanced = CLAHE_contrast_normalization(l_chan, clipLimit=0.2, tileGridSize=(1024,1024))
#         enhanced = cv2.equalizeHist(l_chan)
        
        lab[:, :, 0] = enhanced

        rgb_enhanced[i] = cv2.cvtColor(lab, cv2.COLOR_LAB2RGB)
        
        ax[i, 1].imshow(enhanced, cmap='gray')
        ax[i, 1].set_title('Contrast Enhanced Image ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')
        
        ax[i, 2].imshow(rgb_enhanced[i])
        ax[i, 2].set_title('Contrast Enhanced Image ' + str(i + 1), fontsize=15)
        ax[i, 2].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/clahe_contrast_enhancing.png", dpi=my_dpi)


    

In [12]:

    

if vis == 1:
    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 3
    row_num = 6

    # Plot the result
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))
    hls = {}
    h_channel = {}
    l_channel = {}
    s_channel = {}

    for i in range(6):
        hls = cv2.cvtColor(warped[i], cv2.COLOR_RGB2HLS).astype(np.float)
        h_channel[i] = hls[:, :, 0]
        l_channel[i] = hls[:, :, 1]
        s_channel[i] = hls[:, :, 2]

        ax[i, 0].imshow(h_channel[i], cmap='gray')
        ax[i, 0].set_title('H Channel - Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(l_channel[i], cmap='gray')
        ax[i, 1].set_title('L Channel - Test Image ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

        ax[i, 2].imshow(s_channel[i], cmap='gray')
        ax[i, 2].set_title('S Channel - Test Image ' + str(i + 1), fontsize=15)
        ax[i, 2].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/hls.png", dpi=my_dpi)


    

In [13]:

    

if vis == 1:
    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 4
    row_num = 6

    # Plot the result
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))
    yuv = {}
    y_channel = {}
    u_channel = {}
    v_channel = {}
    u_v_channel = {}

    for i in range(6):
        yuv = cv2.cvtColor(warped[i], cv2.COLOR_RGB2YUV).astype(np.float)
        y_channel[i] = yuv[:, :, 0]
        u_channel[i] = yuv[:, :, 1]
        v_channel[i] = yuv[:, :, 2]
        u_v_channel[i] = u_channel[i] - v_channel[i]
        
        ax[i, 0].imshow(y_channel[i], cmap='gray')
        ax[i, 0].set_title('Y Channel - Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(u_channel[i], cmap='gray')
        ax[i, 1].set_title('U Channel - Test Image ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

        ax[i, 2].imshow(v_channel[i], cmap='gray')
        ax[i, 2].set_title('V Channel - Test Image ' + str(i + 1), fontsize=15)
        ax[i, 2].axis('off')

        ax[i, 3].imshow(u_v_channel[i], cmap='gray')
        ax[i, 3].set_title('U-V Channel - Test Image ' + str(i + 1), fontsize=15)
        ax[i, 3].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/yuv.png", dpi=my_dpi)


    

In [14]:

    

import numpy as np

def color_mask_hls(hls_img):
    # Hue(0-179), Lightning(0-255), Saturation(0-255)
    lower_yellow = np.array([19, 89, 55])
    upper_yellow = np.array([33, 242, 255])
    
    lower_white = np.array([0, 200, 0])
    upper_white = np.array([179, 255, 255])

#     # Hue(0-179), Lightning(0-255), Saturation(0-255)
#     lower_yellow = np.array([20, 89, 100])
#     upper_yellow = np.array([30, 242, 255])
    
#     lower_white = np.array([0, 204, 0])
#     upper_white = np.array([179, 255, 70])

    mask_yellow = cv2.inRange(hls_img, lower_yellow, upper_yellow)
    mask_white = cv2.inRange(hls_img, lower_white, upper_white)

    mask = cv2.bitwise_or(mask_yellow, mask_white)
    
    # Scaling
    scaled_mask = np.uint8(mask/255)
    
    return scaled_mask

def color_mask_yuv(yuv_img):

    y_channel = yuv_img[:, :, 0]
    u_channel = yuv_img[:, :, 1]
    v_channel = yuv_img[:, :, 2]
    u_v_channel = u_channel - v_channel

    # Hue(0-179), Lightning(0-255), Saturation(0-255)
    lower_yellow = np.array([25])
    upper_yellow = np.array([255])
  
    mask_yellow = cv2.inRange(u_v_channel, lower_yellow, upper_yellow)
    
    # Scaling
    scaled_mask = np.uint8(mask_yellow/255)
    
    return scaled_mask

def lane_color_filter(img):
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS).astype(np.float)
    yuv = cv2.cvtColor(img, cv2.COLOR_RGB2YUV).astype(np.float)
    mask = cv2.bitwise_or(color_mask_hls(hls), color_mask_yuv(yuv))

    fltrd_img = cv2.bitwise_and(img, img, mask=mask)
   
    return fltrd_img

if vis == 1:
    # Visualization
    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 3
    row_num = 6

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    for i in range(6):
        filt_img = lane_color_filter(rgb_enhanced[i])

        hls = cv2.cvtColor(rgb_enhanced[i], cv2.COLOR_RGB2HLS).astype(np.float)
        yuv = cv2.cvtColor(warped[i], cv2.COLOR_RGB2YUV).astype(np.float)
        
        mask_img = cv2.bitwise_or(color_mask_hls(hls), color_mask_yuv(yuv))
        
        ax[i, 0].imshow(warped[i])
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(filt_img)
        ax[i, 1].set_title('Color Filtered Result ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

        ax[i, 2].imshow(mask_img,cmap='gray')
        ax[i, 2].set_title('Mask ' + str(i + 1), fontsize=15)
        ax[i, 2].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/color_filtered_images.png", dpi=my_dpi)


    

In [15]:

    

def abs_sobel_thresh(img, orient='x', sobel_kernel=3, thresh=(0, 255)):
    # Apply x or y gradient with the OpenCV Sobel() function
    # and take the absolute value
    if orient == 'x':
        abs_sobel = np.absolute(cv2.Sobel(img, cv2.CV_64F, 1, 0, ksize=sobel_kernel))
    if orient == 'y':
        abs_sobel = np.absolute(cv2.Sobel(img, cv2.CV_64F, 0, 1, ksize=sobel_kernel))
    # Rescale back to 8 bit integer
    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
    # Create a copy and apply the threshold
    binary_output = np.zeros_like(scaled_sobel)
    binary_output[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1

    # Return the result
    return binary_output

if vis == 1:
    # Visualization

    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 3
    row_num = 6

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))


    s_thresh = (20, 255)
    kernel = 11

    for i in range(6):
        hls = cv2.cvtColor(rgb_enhanced[i], cv2.COLOR_RGB2HLS).astype(np.float)
        h_channel = hls[:, :, 0]
        l_channel = hls[:, :, 1]
        s_channel = hls[:, :, 2]

        sobelx_img = abs_sobel_thresh(l_channel, orient='x', 
                                      sobel_kernel=kernel, thresh=s_thresh)
        sobely_img = abs_sobel_thresh(l_channel, orient='y', 
                                      sobel_kernel=kernel, thresh=s_thresh)

        ax[i, 0].imshow(warped[i])
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(sobelx_img, cmap='gray')
        ax[i, 1].set_title('Abs Sobel Filter x ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

        ax[i, 2].imshow(sobely_img, cmap='gray')
        ax[i, 2].set_title('Abs Sobel Filter y ' + str(i + 1), fontsize=15)
        ax[i, 2].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/abs_sobel_thresh_images.png", dpi=my_dpi)


    

In [16]:

    

if vis == 1:
    # Visualization
    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 3
    row_num = 6

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    s_threshl = (12, 255)
    s_threshs = (8, 255)
    kernel = 1

    for i in range(6):
        hls = cv2.cvtColor(rgb_enhanced[i], cv2.COLOR_RGB2HLS).astype(np.float)
        h_channel = hls[:, :, 0]
        l_channel = hls[:, :, 1]
        s_channel = hls[:, :, 2]

        sobell_img = abs_sobel_thresh(l_channel, orient='x', 
                                      sobel_kernel=kernel, thresh=s_threshl)
        sobels_img = abs_sobel_thresh(s_channel, orient='x', 
                                      sobel_kernel=kernel, thresh=s_threshs)

        ax[i, 0].imshow(rgb_enhanced[i])
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(sobell_img, cmap='gray')
        ax[i, 1].set_title('Abs Sobel Filter x - L chan ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

        ax[i, 2].imshow(sobels_img, cmap='gray')
        ax[i, 2].set_title('Abs Sobel Filter x - S chan' + str(i + 1), fontsize=15)
        ax[i, 2].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/abs_sobel_thresh_ls_images.png", dpi=my_dpi)


    

In [17]:

    

def dir_threshold(img, sobel_kernel=3, thresh=(0, np.pi/2)):
    # Calculate the x and y gradients
    sobelx = cv2.Sobel(img, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobely = cv2.Sobel(img, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    # Take the absolute value of the gradient direction,
    # apply a threshold, and create a binary image result
    absgraddir = np.arctan2(np.absolute(sobely), np.absolute(sobelx))
    binary_output = np.zeros_like(absgraddir, dtype=np.uint8)
    binary_output[(absgraddir >= thresh[0]) & (absgraddir <= thresh[1])] = 1   
    
    # Return the binary image
    return binary_output

if vis == 1:
    # Visualization

    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 4
    row_num = 6

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    s_thresh = (0.25, 0.4)
    kernel = 1

    for i in range(6):
        blur = gaussian_blur(warped[i], kernel_size=5)

        hls = cv2.cvtColor(blur, cv2.COLOR_RGB2HLS).astype(np.float)
        h_channel = hls[:, :, 0]
        l_channel = hls[:, :, 1]
        s_channel = hls[:, :, 2]

        sobelh_img = dir_threshold(h_channel, sobel_kernel=kernel, thresh=s_thresh )
        sobell_img = dir_threshold(l_channel, sobel_kernel=kernel, thresh=s_thresh )
        sobels_img = dir_threshold(s_channel, sobel_kernel=kernel, thresh=s_thresh )

        ax[i, 0].imshow(warped[i])
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(sobelh_img, cmap='gray')
        ax[i, 1].set_title('Direction Sobel Filter x - h chan ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

        ax[i, 2].imshow(sobell_img, cmap='gray')
        ax[i, 2].set_title('Direction Sobel Filter x - L chan ' + str(i + 1), fontsize=15)
        ax[i, 2].axis('off')

        ax[i, 3].imshow(sobels_img, cmap='gray')
        ax[i, 3].set_title('Direction Sobel Filter x - S chan' + str(i + 1), fontsize=15)
        ax[i, 3].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/direction_sobel_thresh_s_vs_l_images.png", dpi=my_dpi)


    

In [18]:

    

def mag_thresh(img, sobel_kernel=3, thresh=(0, 255)):
    # Take both Sobel x and y gradients
    sobelx = cv2.Sobel(img, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobely = cv2.Sobel(img, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    # Calculate the gradient magnitude
    gradmag = np.sqrt(sobelx**2 + sobely**2)
    # Rescale to 8 bit
    scale_factor = np.max(gradmag)/255
    gradmag = (gradmag/scale_factor).astype(np.uint8)
    # Create a binary image of ones where threshold is met, zeros otherwise
    binary_output = np.zeros_like(gradmag)
    binary_output[(gradmag >= thresh[0]) & (gradmag <= thresh[1])] = 1

    # Return the binary image
    return binary_output

if vis == 1:
    # Visualization

    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 3
    row_num = 6

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    for i in range(6):
        hls = cv2.cvtColor(rgb_enhanced[i], cv2.COLOR_RGB2HLS).astype(np.float)
        h_channel = hls[:, :, 0]
        l_channel = hls[:, :, 1]
        s_channel = hls[:, :, 2]

        sobell_img = mag_thresh(l_channel, sobel_kernel=11, thresh=(17, 255))
        sobels_img = mag_thresh(s_channel, sobel_kernel=11, thresh=(15, 255))

        ax[i, 0].imshow(warped[i])
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(sobell_img, cmap='gray')
        ax[i, 1].set_title('Magnitude Sobel Filter x - L chan ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

        ax[i, 2].imshow(sobels_img, cmap='gray')
        ax[i, 2].set_title('Magnitude Sobel Filter x - S chan' + str(i + 1), fontsize=15)
        ax[i, 2].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/magnitude_sobel_thresh_s_vs_l_images.png", dpi=my_dpi)


    

In [19]:

    

# Choose points to define the region of interest
# leftx = np.array([0, 280, 380])
# lefty = np.array([0, 360, 720])
# rightx = np.array([1280, 1000, 900])
# righty = np.array([0, 360, 720])

leftx = np.array([200, 315, 350])
lefty = np.array([0, 360, 720])
rightx = np.array([1080, 965, 930])
righty = np.array([0, 360, 720])

# leftx = np.array([0, 200, 300, 350])
# lefty = np.array([150, 260, 500, 720])

# rightx = np.array([1280, 1080, 980, 930])
# righty = np.array([150, 260, 500, 720])

# Fit a second order polynomial to each
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)

# Generate polynomials x y points
ploty = np.linspace(0, 679, 680 )
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]

# And recast the x and y points into usable format for cv2.fillPoly()
left_line = np.array([np.transpose(np.vstack([left_fitx, ploty]))])   
right_line = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
line_pts = np.hstack((left_line, right_line))

def region_of_interest(img, line_pts):
    """
    Applies an image mask.
    
    Only keeps the region of the image defined by the polygon
    formed from `vertices`. The rest of the image is set to black.
    """
    
    #defining a blank mask to start with
    mask = np.zeros_like(img)   
    
    #defining a 3 channel or 1 channel color to fill the mask with depending on the input image
    if len(img.shape) > 2:
        channel_count = img.shape[2]  # i.e. 3 or 4 depending on your image
        ignore_mask_color = (255,) * channel_count
    else:
        ignore_mask_color = 255

    #filling pixels inside the polygon defined by "vertices" with the fill color    
    cv2.fillPoly(mask, np.int_([line_pts]), ignore_mask_color)
    
    #returning the image only where mask pixels are nonzero
    masked_image = cv2.bitwise_and(img, mask)
    
    return masked_image

if vis == 1:
    # Visualization

    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 2
    row_num = 6

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    for i in range(6):
        croped_img = region_of_interest(warped[i], line_pts)

        ax[i, 0].imshow(warped[i])
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(croped_img, cmap='gray')
        ax[i, 1].set_title('Cropped Image ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

        for j in range(3):
            ax[i, 0].plot(leftx[j], lefty[j], 'rs')
            ax[i, 1].plot(leftx[j], lefty[j], 'rs')
            ax[i, 0].plot(rightx[j], righty[j], 'rs')
            ax[i, 1].plot(rightx[j], righty[j], 'rs')

    f.tight_layout()
    plt.savefig("./output_images/roi_filter.png", dpi=my_dpi)


    

In [20]:

    

def bin_image_pipeline(img, line_pts):
    
    # Apply a gaussian blur to smooth out 
#     img = gaussian_blur(img, kernel_size=5)
    
    # Apply the best working thresholding functions
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS).astype(np.float)
    yuv = cv2.cvtColor(img, cv2.COLOR_RGB2YUV).astype(np.float)
    col_mask_bin = cv2.bitwise_or(color_mask_hls(hls), color_mask_yuv(yuv))
    
    # Convert to HLS color space
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)

    enhanced = CLAHE_contrast_normalization(hls[:, :, 1], clipLimit=0.2, tileGridSize=(1024,1024))
    
    hls[:, :, 1] = enhanced
    
    h_channel = hls[:, :, 0]
    l_channel = hls[:, :, 1]
    s_channel = hls[:, :, 2]
    
    # 6 ?
    gradxl_hd = abs_sobel_thresh(l_channel, orient='x', sobel_kernel=11, thresh=(12, 255))
    gradxs_hd = abs_sobel_thresh(s_channel, orient='x', sobel_kernel=11, thresh=(8, 255))

#     gradxl = abs_sobel_thresh(l_channel, orient='x', sobel_kernel=11, thresh=(50, 255))
#     gradxs = abs_sobel_thresh(s_channel, orient='x', sobel_kernel=11, thresh=(50, 255))
    
#     dir_gradl = dir_threshold(l_channel, sobel_kernel=1, thresh=(0.25, 0.4) )
#     dir_grads = dir_threshold(s_channel, sobel_kernel=1, thresh=(0.25, 0.4) )
    
#     mag_gradl = mag_thresh(l_channel, sobel_kernel=11, thresh=(17, 255))
#     mag_grads = mag_thresh(s_channel, sobel_kernel=11, thresh=(15, 255))
    
    gradx_hd = cv2.bitwise_or(gradxl_hd, gradxs_hd)
    combined1 = cv2.bitwise_and(gradx_hd, col_mask_bin)
    
#     dir_grad = cv2.bitwise_or(dir_gradl, dir_grads)
#     mag_grad = cv2.bitwise_or(mag_gradl, mag_grads)
#     combined2 = cv2.bitwise_and(dir_gradl, mag_grad)
    
#     gradx = cv2.bitwise_or(gradxl, gradxs)
#     combined3 = cv2.bitwise_or(combined1, combined2, gradx)

    croped_img = region_of_interest(combined1, line_pts)
    
    return croped_img

if vis == 1:
    #Visualize

    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 2
    row_num = 6

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))
    col_bin_img = {}
    comb_img = {}

    for i in range(6):
        comb_img[i] = bin_image_pipeline(warped[i],
                                         line_pts)

        ax[i, 0].imshow(warped[i])
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(comb_img[i], cmap='gray')
        ax[i, 1].set_title('Combined Binary Image ' + str(i + 1), fontsize=15)
        ax[i, 1].axis('off')

    f.tight_layout()
    plt.savefig("./output_images/thresholded_binary_images.png", dpi=my_dpi)


    

In [21]:

    

# Polynomial Regression
def polyfit(x, y, degree):
    
    coeffs = np.polyfit(x, y, degree)

    # r-squared
    p = np.poly1d(coeffs)
    # fit values, and mean
    yhat = p(x)                      # or [p(z) for z in x]
    ybar = np.sum(y)/len(y)          # or sum(y)/len(y)
    ssreg = np.sum((yhat-ybar)**2)   # or sum([ (yihat - ybar)**2 for yihat in yhat])
    sstot = np.sum((y - ybar)**2)    # or sum([ (yi - ybar)**2 for yi in y])
    r2 = ssreg / sstot

    return coeffs, r2


    

In [22]:

    

def sliding_window_search(binary_warped, nwindows, margin, minpix):
    # Assuming you have created a warped binary image called "binary_warped"
    # Take a histogram of the bottom half of the image
    histogram = np.sum(binary_warped[int(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))*255
    # 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

    # Set height of windows
    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 for each window
    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 = []

    # empty list of current detection windows centers..
    leftx_w_center = []
    rightx_w_center = []
    y_w_center = []
    
    # empty list of detection status
    left_detected = []
    right_detected = []
    
    # 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
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin
        win_xright_low = rightx_current - margin
        win_xright_high = rightx_current + margin
        
        # append the current window center to the lists
        leftx_w_center.append(leftx_current)
        rightx_w_center.append(rightx_current)
        y_w_center.append(int((win_y_high + win_y_low)/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 on their mean position
        if len(good_left_inds) > minpix:
            # Draw the windows on the visualization image in green
            cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),(0,255,0), 2)
            left_detected.append('True')
            leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
        else:
            # Draw the windows on the visualization image in red
            cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),(255,0,0), 2)
            left_detected.append('False')
        
        if len(good_right_inds) > minpix:
            # Draw the windows on the visualization image in green
            cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 2)
            right_detected.append('True')
            rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
        else:
            # Draw the windows on the visualization image in red
            cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(255,0,0), 2)
            right_detected.append('False')

    # Concatenate the arrays of indices
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)

    # 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] 

    # Include lane pixels in "out image"
    out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [244, 238, 66]
    out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [66, 158, 244]
    
    # Fit a second order polynomial to each
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    
#     return out_img
    return left_fit, right_fit, out_img
#     return left_fit, right_fit, out_img, histogram


    

In [23]:

    

if vis == 1:
    #Visualize

    # Choose the number of sliding windows
    nwindows = 20
    # Set the width of the windows +/- margin
    margin = 50
    # Set minimum number of pixels found to recenter window
    minpix = 15

    x_pix = 1200  # x dimension of the resulting figure 
    col_num = 2
#     col_num = 3
    row_num = 6

    # Plot the result of applying the pipeline of undistorded images
    f, ax = plt.subplots(row_num, col_num, 
                         figsize=(x_pix/my_dpi, x_pix/col_num*row_num/aspect_ratio/my_dpi))

#     f, ax = plt.subplots(row_num, col_num, 
#                          figsize=(x_pix/my_dpi*1.55, x_pix/col_num*row_num/aspect_ratio/my_dpi))

    left_fit = {}
    right_fit = {}
    out_img = {}

    for i in range(6):

#         # Get lane pixels
#         out_img[i]    = sliding_window_search(comb_img[i],
#                                                 nwindows,
#                                                 margin,
#                                                 minpix)
        
        # Get lane pixels
        left_fit[i],  \
        right_fit[i], \
        out_img[i]    = sliding_window_search(comb_img[i],
                                                nwindows,
                                                margin,
                                                minpix)
        
#         left_fit[i],  \
#         right_fit[i], \
#         out_img[i],   \
#         histogram     = sliding_window_search(comb_img[i],
#                                                 nwindows,
#                                                 margin,
#                                                 minpix)

        # Generate x and y values for polyfit plotting
        ploty = np.linspace(0, comb_img[i].shape[0]-1, comb_img[i].shape[0] )
        left_fitx = left_fit[i][0]*ploty**2 + left_fit[i][1]*ploty + left_fit[i][2]
        right_fitx = right_fit[i][0]*ploty**2 + right_fit[i][1]*ploty + right_fit[i][2]

        ax[i, 0].imshow(comb_img[i], cmap="gray")
        ax[i, 0].set_title('Test Image ' + str(i + 1), fontsize=15)
        ax[i, 0].axis('off')

        ax[i, 1].imshow(out_img[i])
        ax[i, 1].set_title('Slidding Window Search ' + str(i + 1), fontsize=15)
        ax[i, 1].plot(left_fitx, ploty, color='yellow')
        ax[i, 1].plot(right_fitx, ploty, color='yellow')
        ax[i, 1].set_xlim(0, 1280)
        ax[i, 1].set_ylim(720, 0)
        ax[i, 1].axis('off')
        
#         ax[i, 2].plot(histogram)
#         ax[i, 2].set_title('Histogram ' + str(i + 1), fontsize=15)
#         ax[i, 2].set_xlabel('Position')
#         ax[i, 2].set_ylabel('Pixel count')

    f.tight_layout()
    plt.savefig("./output_images/slidding_window_search.png", dpi=my_dpi)


    

In [52]:

    

xm_per_pix = 3.65/400  # meters per pixel in x dimension
ym_per_pix = 3/100 # meters per pixel in y dimension

def get_poly_m(left_fit, right_fit):

    ploty = np.linspace(0, 719, num=720)
    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]
    
    # Fit new polynomials to x,y in world space
    left_fit_m = np.polyfit(ploty*ym_per_pix, left_fitx*xm_per_pix, 2)
    right_fit_m = np.polyfit(ploty*ym_per_pix, right_fitx*xm_per_pix, 2)
    
    return left_fit_m, right_fit_m


    

In [53]:

    

def get_lane_curvature(left_fit_m, right_fit_m):
    y_eval = 719  #bottom of the image
    
    # Calculate the radii of curvature
    left_curverad = (((1 + (2*left_fit_m[0]*y_eval*ym_per_pix + left_fit_m[1])**2)**1.5) / 
                    np.absolute(2*left_fit_m[0]))
    right_curverad = (((1 + (2*right_fit_m[0]*y_eval*ym_per_pix + right_fit_m[1])**2)**1.5) / 
                    np.absolute(2*right_fit_m[0]))

    return left_curverad, right_curverad

if vis == 1:
    for i in range(6):
        left_fit_m, right_fit_m = get_poly_m(left_fit[i], right_fit[i])

        left_curverad, right_curverad = get_lane_curvature(left_fit_m, right_fit_m)

        print(i, 'test image', left_curverad, 'm', right_curverad, 'm')


    

    




0 test image 411.8339771 m 599.293713658 m
1 test image 461.466842995 m 456.985167912 m
2 test image 963.653150969 m 634.116216771 m
3 test image 1133.96093287 m 203.797936493 m
4 test image 296.965987192 m 2554.87057954 m
5 test image 1063.02318549 m 689.721370075 m

In [54]:

    

def distance_from_center(left_fit_m, right_fit_m):

    y_eval = 719 * ym_per_pix  #bottom of the image (1280x720)
        
    leftx = left_fit_m[0]*y_eval**2 + left_fit_m[1]*y_eval + left_fit_m[2]
    rightx = right_fit_m[0]*y_eval**2 + right_fit_m[1]*y_eval + right_fit_m[2]
    
    xcenter = 639*xm_per_pix  #center of the image (1280x720)
    
    dist_off_center = xcenter - (leftx + rightx)/2
    
    lane_width = rightx - leftx
    
    return dist_off_center, lane_width

if vis == 1:
    for i in range(6):
        left_fit_m, right_fit_m = get_poly_m(left_fit[i], right_fit[i])

        dist_off_center, lane_width = distance_from_center(left_fit_m, right_fit_m)

        print(i, ' distance from center ', dist_off_center, 'm', ' lane width ',lane_width)


    

    




0  distance from center  -0.170094538524 m  lane width  3.69951418502
1  distance from center  -0.410575503411 m  lane width  3.85726239329
2  distance from center  -0.0968099815003 m  lane width  3.70072009767
3  distance from center  -0.322133391714 m  lane width  3.85636558166
4  distance from center  0.031163404307 m  lane width  3.87664198227
5  distance from center  -0.224115538474 m  lane width  3.78923177889

In [55]:

    

def lane_tracking(binary_warped, margin, nwindows, minpix, left_fit_1, right_fit_1, smoothing, skiped_n_1, lane_width):
    
    reset = 0
    
    # Assume you now have a new warped binary image 
    # from the next frame of video (also called "binary_warped")
    nonzero = binary_warped.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])

    left_lane_inds = ((nonzerox > (left_fit_1[0]*(nonzeroy**2) + left_fit_1[1]*nonzeroy + left_fit_1[2] - margin))
                      & (nonzerox < (left_fit_1[0]*(nonzeroy**2) + left_fit_1[1]*nonzeroy + left_fit_1[2] + margin))) 
    right_lane_inds = ((nonzerox > (right_fit_1[0]*(nonzeroy**2) + right_fit_1[1]*nonzeroy + right_fit_1[2] - margin))
                       & (nonzerox < (right_fit_1[0]*(nonzeroy**2) + right_fit_1[1]*nonzeroy + right_fit_1[2] + margin)))  

    # 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]
    
    #################
    # SANITY CHECKS
    #################

    # Sanity check: if there is pixels detected for both lines AND
    # Lane width is within specs AND..
    if (len(rightx) > 0 and len(leftx) > 0):
        # Fit a second order polynomial to each
        left_fit, left_r2 = polyfit(lefty, leftx, 2)
        right_fit, right_r2 = polyfit(righty, rightx, 2)
        
        # Get polynomial fit in "meter space"
        left_fit_m, right_fit_m = get_poly_m(left_fit, right_fit)

        # Get distance and lane width in meters
        dist_off_center, lane_width = distance_from_center(left_fit_m, right_fit_m)

        #Lane width tolerance
        tol = 3.55 * 0.17        
        if (lane_width >= 3.55-tol and lane_width <= 3.55+tol):

            # Compute the difference between this frame's fit and last frame's fit
            left_fit_diff =  abs(left_fit - left_fit_1)
            right_fit_diff = abs(right_fit - right_fit_1)
            
            # If the fit change is within the limits
            if (left_fit_diff[0]  < 7.5e-4 and left_fit_diff[1]  < 9e-1 and left_fit_diff[2]  < 40 and
                right_fit_diff[0] < 7.5e-4 and right_fit_diff[1] < 9e-1 and right_fit_diff[2] < 40):
                #Filter
                # Blending ratio
                # 1.0 -> ignore completely the other line
                # 0.5 -> rely on both lines equaly
                # 0.0 -> rely only on the other line
                ratio_blend = 0.95

                # Blend the left curve with the right curve
                left_fit[0] = left_fit[0] * ratio_blend + right_fit[0] * (1 - ratio_blend)
                left_fit[1] = left_fit[1] * ratio_blend + right_fit[1] * (1 - ratio_blend)

                # Blend the right curve with the left curve
                right_fit[0] = right_fit[0] * ratio_blend + left_fit[0] * (1 - ratio_blend)
                right_fit[1] = right_fit[1] * ratio_blend + left_fit[1] * (1 - ratio_blend)

                # Reset skip counter
                skiped_n = 0
            else:
                # Increment skip counter
                skiped_n = skiped_n_1 + 1
                left_fit = left_fit_1
                right_fit = right_fit_1
                left_r2 = 0
                right_r2 = 0
                left_fit_diff = left_fit_1 - left_fit_1
                right_fit_diff = right_fit_1 - right_fit_1
        else:
            # Increment skip counter
            skiped_n = skiped_n_1 + 1
            left_fit = left_fit_1
            right_fit = right_fit_1
            left_r2 = 0
            right_r2 = 0
            left_fit_diff = left_fit_1 - left_fit_1
            right_fit_diff = right_fit_1 - right_fit_1
    else:
        # Increment skip counter
        skiped_n = skiped_n_1 + 1
        left_fit = left_fit_1
        right_fit = right_fit_1
        left_r2 = 0
        right_r2 = 0
        left_fit_diff = left_fit_1 - left_fit_1
        right_fit_diff = right_fit_1 - right_fit_1

    # If more than 50 frames have been skiped redo a sliding window search...
    if skiped_n > 50:
        # Apply the 1rst iteration sliding window search
        left_fit, right_fit, out_img  = sliding_window_search(binary_warped, nwindows, margin, minpix)
        skiped_n = 50
    
    # Low Pass filtering
    left_fit = left_fit * 1/smoothing + left_fit_1 * (1 - 1/smoothing)
    right_fit = right_fit * 1/smoothing + right_fit_1 * (1 - 1/smoothing)    

    # If tracking is used, draw the out_img..
    if skiped_n <= 50:
        # Generate x and y values for plotting
        ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
        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]

        # 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[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.3, 0)
    
    return left_fit, right_fit, left_r2, right_r2, out_img, skiped_n, left_fit_diff, right_fit_diff