In [2]:
import cv2
import glob
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
from moviepy.editor import VideoFileClip
import numpy as np
from IPython.display import HTML
%matplotlib inline
print("Dependencies Loaded!")
First lets find the corners of all the chessboard calibration images
In [4]:
CALIBRATION_FILES = glob.glob("./camera_cal/calibration*.jpg")
CHESS_X = 9
CHESS_Y = 6
IMAGE_POINTS = []
OBJECT_POINTS = []
CHESSBOARD = np.zeros((CHESS_X*CHESS_Y, 3), np.float32)
CHESSBOARD[:,:2] = np.mgrid[0:CHESS_X,0:CHESS_Y].T.reshape(-1,2)
for file in CALIBRATION_FILES:
img = mpimg.imread(file)
# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray, (CHESS_X,CHESS_Y), None)
if ret == True:
IMAGE_POINTS.append(corners)
OBJECT_POINTS.append(CHESSBOARD)
# Draw all the chessboard corners
img = cv2.drawChessboardCorners(img, (CHESS_X,CHESS_Y), corners, ret)
plt.imshow(img)
Then let's calibrate the camera with these points. We should see the chessboard's distortion at the edges of the camera uncurl
In [5]:
img = mpimg.imread("./camera_cal/calibration1.jpg")
ret, MTX, DIST, rvecs, tvecs = cv2.calibrateCamera(
OBJECT_POINTS,
IMAGE_POINTS,
img.shape[0:2],
None,
None
)
def plot_side_by_side(img1, img2, label1="Original Image", label2="New Image", should_mask=False):
# Helper function to plot two images side by side
if should_mask:
i1 = mask(img1)
i2 = mask(img2)
else:
i1 = img1
i2 = img2
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(i1)
ax1.set_title(label1, fontsize=50)
ax2.imshow(i2)
ax2.set_title(label2, fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
def undistort(img):
# Undistor an image from the camera calibration we did earlier
return cv2.undistort(img, MTX, DIST, None, MTX)
plot_side_by_side(img, undistort(img), "Distorted", "Undistorted")
In [7]:
X, Y = (1280, 720)
MID_X, MID_Y = int(X/2), int(Y/2)
X_OFF, Y_OFF = 75, 93
MASK = np.array([
[(0, Y), (X, Y),
(MID_X+X_OFF, MID_Y+Y_OFF), (MID_X-X_OFF, MID_Y+Y_OFF)]
])
def mask(img):
# Apply a mask to the image for the part of the road that we care about
mask = np.zeros_like(img)
cv2.fillPoly(mask, MASK, [255, 255, 255])
masked_image = cv2.bitwise_and(img, mask)
return masked_image
img = mpimg.imread("./test_images/test1.jpg")
plot_side_by_side(img, mask(img), label2="Masked")
In [8]:
img = mpimg.imread("./test_images/straight_lines1.jpg")
BIRDS_EYE = np.float32([
[320 , 720],
[960 , 720],
[960 , 0],
[320 , 0]
])
def birds_eye(img):
# Get a transform matrix from the masked region to the birds eye view
M = cv2.getPerspectiveTransform(np.float32(MASK[0]), BIRDS_EYE)
# Apply the warp matrix
warped = cv2.warpPerspective(mask(img), M, (img.shape[1], img.shape[0]), flags=cv2.INTER_LINEAR)
return warped
def fps(img):
# Get a transform matrix from the birds eye view to the masked region of interest
M = cv2.getPerspectiveTransform(BIRDS_EYE, np.float32(MASK[0]))
# Apply the matrix to unwarp the birds eye view to the road view
unwarped = cv2.warpPerspective(img, M, (img.shape[1], img.shape[0]), flags=cv2.INTER_LINEAR)
return unwarped
plot_side_by_side(mask(img), birds_eye(img))
In [9]:
def abs_sobel_thresh(img, orient="x", ksize=3, thresh=(0, 255)):
# Transform to HLS color space, and use the S portion
S = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)[:,:,2]
# Apply the proper Sobel gradient
if orient == "x":
sobel = cv2.Sobel(S, cv2.CV_64F, 1, 0, ksize=ksize)
else:
sobel = cv2.Sobel(S, cv2.CV_64F, 0, 1, ksize=ksize)
# Get the absolute value of the sobel and scale it
abs_sobel = np.absolute(sobel)
scaled = np.uint8(255 * abs_sobel / np.max(abs_sobel))
# Apply a mask for a bianry value
binary = np.zeros_like(scaled)
binary[(scaled >= thresh[0]) & (scaled <= thresh[1])] = 1
return binary
def mag_thresh(img, ksize=3, thresh=(0, 255)):
# Transform to HLS color space, and use the S portion
S = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)[:,:,2]
# Grab the sobel values for both axis
sobel_x = cv2.Sobel(S, cv2.CV_64F, 1, 0, ksize=ksize)
sobel_y = cv2.Sobel(S, cv2.CV_64F, 0, 1, ksize=ksize)
# Calculate the magnitude (distance) of the sobel at x and y
mag = np.sqrt(sobel_x ** 2 + sobel_y ** 2)
# Scale magnitude
scaled = np.uint8(255 * mag / np.max(mag))
# Apply a mask for a binary value
binary = np.zeros_like(scaled)
binary[(scaled >= thresh[0]) & (scaled <= thresh[1])] = 1
return binary
def dir_thresh(img, ksize=3, thresh=(0, np.pi / 2)):
# Transform to HLS color space, and use the S portion
S = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)[:,:,2]
# Grab the sobel values for both axis
sobel_x = cv2.Sobel(S, cv2.CV_64F, 1, 0, ksize=ksize)
sobel_y = cv2.Sobel(S, cv2.CV_64F, 0, 1, ksize=ksize)
# Get absolute value of x and y sobels
abs_x = np.absolute(sobel_x)
abs_y = np.absolute(sobel_y)
# Get arctangent for direction
arc = np.arctan2(abs_y, abs_x)
# Apply a mask for a binary value
binary = np.zeros_like(arc)
binary[(arc >= thresh[0]) & (arc <= thresh[1])] = 1
return binary
def sobel_comp(img, plot=False):
# Calculate various sobel binary images
lanes_x = abs_sobel_thresh(img, orient="x", ksize=31, thresh=(30, 200))
lanes_y = abs_sobel_thresh(img, orient="y", ksize=31, thresh=(50, 200))
lanes_mag = mag_thresh(img, ksize=31, thresh=(50, 250))
lanes_dir = dir_thresh(img, ksize=31, thresh=(0.7, 1.3))
# Calculate a composition of the above sobels
lanes = np.zeros_like(lanes_x)
lanes[
((lanes_x == 1) & (lanes_y == 1)) &
((lanes_mag == 1) & (lanes_dir == 1))
] = 1
if plot:
plot_side_by_side(img, lanes_x, label2="Sobel X", should_mask=True)
plot_side_by_side(img, lanes_y, label2="Sobel Y", should_mask=True)
plot_side_by_side(img, lanes_mag, label2="Sobel Magnitude", should_mask=True)
plot_side_by_side(img, lanes_dir, label2="Sobel Direction", should_mask=True)
plot_side_by_side(img, lanes, label2="Composite", should_mask=True)
return lanes
img = mpimg.imread("./test_images/test4.jpg")
sobel_comp(img, plot=True)
Out[9]:
In [10]:
def r_thresh(img, thresh=(200,255)):
# Get R values
R = img[:,:,0]
bin_r = np.zeros_like(R)
# Apply a mask to get a binary image of thresholded R values
bin_r[(R > thresh[0]) & (R <= thresh[1])] = 1
return bin_r
def h_thresh(hls, thresh=(15,100)):
# Get H value from hls color space image
H = hls[:,:,0]
bin_h = np.zeros_like(H)
# Apply a mask to get a binary image of thresholded H values
bin_h[(H > thresh[0]) & (H <= thresh[1])] = 1
return bin_h
def l_thresh(hls, thresh=(15,100)):
# Get L value from hls color space image
L = hls[:,:,0]
bin_l = np.zeros_like(L)
# Apply a mask to get a binary image of thresholded L values
bin_l[(L > thresh[0]) & (L <= thresh[1])] = 1
return bin_l
def s_thresh(hls, thresh=(90, 255)):
# Get S value from hls color space image
S = hls[:,:,2]
bin_s = np.zeros_like(S)
# Apply a mask to get a binary image of thresholded S values
bin_s[(S > thresh[0]) & (S <= thresh[1])] = 1
return bin_s
def v_thresh(hsv, thresh=(100,255)):
# Get V value from hsv color space image
V = hsv[:,:,2]
bin_v = np.zeros_like(V)
# Apply a mask to get a binary image of thresholded V values
bin_v[(V > thresh[0]) & (V <= thresh[1])] = 1
return bin_v
def colors_comp(img, plot=False):
hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)
# Grab thresholded binary images for H, L, and S from HLS color space
bin_h = h_thresh(hls, thresh=(0, 30))
bin_l = l_thresh(hls, thresh=(0, 50))
bin_s = s_thresh(hls, thresh=(100, 255))
# Apply a mask to compose values
colors = np.zeros_like(img[:,:,0])
colors[
(((bin_h == 1)) &
((bin_l == 1)) &
((bin_s == 1)))
] = 1
if plot:
plot_side_by_side(hls[:,:,0], bin_h, label2="H Channel", should_mask=False)
plot_side_by_side(hls[:,:,1], bin_l, label2="L Channel", should_mask=False)
plot_side_by_side(hls[:,:,2], bin_s, label2="S Channel (HLS)", should_mask=False)
plot_side_by_side(img, colors, label2="Colors Comp", should_mask=True)
return colors
img = mpimg.imread("./test_images/test4.jpg")
colors_comp(img, plot=True)
Out[10]:
In [11]:
def binary_comp(img, plot=False):
# Compose the binary images from sobel gradients and colors
colors = colors_comp(img, plot=plot)
sob = sobel_comp(img, plot=plot)
comp = np.zeros_like(img[:,:,0])
comp[
(colors == 1) | (sob == 1)
] = 1
if plot:
plot_side_by_side(img, comp, label2="Total Comp", should_mask=True)
return comp
def pipeline(img):
# Apply a pipeline to get from a RGB camera view to an overhead binary image of the lanes
# Undistort the image from the camera calibrations
undist = undistort(img)
# Grab a thresholded binary image based on sobel gradients and color thresholding
binary = binary_comp(undist)
# Apply a perspective transform to obtain an overhead view of the road
birds = birds_eye(binary)
return birds
def test_color_thresholds(filename):
img = mpimg.imread(filename)
binary = binary_comp(img, plot=False)
pipe = pipeline(img)
plot_side_by_side(birds_eye(img), birds_eye(binary), label2="Total Comp", should_mask=False)
plot_side_by_side(birds_eye(img), pipe, label2="Pipeline", should_mask=False)
# test_color_thresholds("./test_images/straight_lines1.jpg")
# test_color_thresholds("./test_images/straight_lines2.jpg")
# test_color_thresholds("./test_images/test1.jpg")
test_color_thresholds("./test_images/test2.jpg")
# test_color_thresholds("./test_images/test3.jpg")
# test_color_thresholds("./test_images/test4.jpg")
# test_color_thresholds("./test_images/test5.jpg")
# test_color_thresholds("./test_images/test6.jpg")
In [12]:
def histogram(img):
# Return a histogram of the various values in an image
# Used to find where to first search for lanes
return np.sum(img[img.shape[0]//2:,:], axis=0)
binary = pipeline(img)
hist = histogram(binary)
plt.plot(hist)
Out[12]:
In [15]:
img = mpimg.imread("./test_images/test4.jpg")
binary = pipeline(img)
hist = histogram(binary)
plot_side_by_side(img, binary)
plot_side_by_side(img, binary)
def nonzeros(binary):
# Get nonzero values from an image
nonzero = binary.nonzero()
x = np.array(nonzero[1])
y = np.array(nonzero[0])
return (x, y)
def find_lanes(binary):
stacked = np.dstack((binary, binary, binary)) * 255
# Grab the midpoint
mid = np.int(hist.shape[0] / 2)
# Where to start the left lane search
left_lane_start = np.argmax(hist[:mid])
# Where to start the right lane search
right_lane_start = np.argmax(hist[mid:]) + mid
# How many windows to use for search
windows = 9
# Height of each window used
height = np.int(hist.shape[0] / windows)
nonzero_x, nonzero_y = nonzeros(binary)
left_curr = left_lane_start
right_curr = right_lane_start
# Margin of each windowed search
margin = 100
# Minimum number of pixels to recenter window
min_pix = 50
left_lane = []
right_lane = []
# Search through each window
for window in range(windows):
# Get window bounds
y_lo = binary.shape[0] - (window + 1) * height
y_hi = binary.shape[0] - window * height
l_x_lo = left_curr - margin
l_x_hi = left_curr + margin
r_x_lo = right_curr - margin
r_x_hi = right_curr + margin
# Draw windows
cv2.rectangle(stacked, (l_x_lo, y_lo), (l_x_hi, y_hi), (0, 255, 0), 2)
cv2.rectangle(stacked, (r_x_lo, y_lo), (r_x_hi, y_hi), (0, 255, 0), 2)
# Get nonzero pixels within the window
left = (
(nonzero_y >= y_lo) &
(nonzero_y < y_hi) &
(nonzero_x >= l_x_lo) &
(nonzero_x < l_x_hi)
).nonzero()[0]
right = (
(nonzero_y >= y_lo) &
(nonzero_y < y_hi) &
(nonzero_x >= r_x_lo) &
(nonzero_x < r_x_hi)
).nonzero()[0]
# Append to the lane
left_lane.append(left)
right_lane.append(right)
# If enough lane pixels were found, recenter the window for the next windowed search
if len(left) > min_pix:
left_curr = np.int(np.mean(nonzero_x[left]))
if len(right) > min_pix:
right_curr = np.int(np.mean(nonzero_x[right]))
# Concatenate the pixels
left_lane = np.concatenate(left_lane)
right_lane = np.concatenate(right_lane)
return (left_lane, right_lane)
def fit_polys(binary, lanes):
left_lane, right_lane = lanes
nonzero_x, nonzero_y = nonzeros(binary)
left_x = nonzero_x[left_lane]
left_y = nonzero_y[left_lane]
right_x = nonzero_x[right_lane]
right_y = nonzero_y[right_lane]
# Fit a 2nd order polynomial to each lane
left_poly = np.polyfit(left_y, left_x, 2)
right_poly = np.polyfit(right_y, right_x, 2)
return left_poly, right_poly
def plot_lanes(binary, lanes, polys):
left_lane, right_lane = lanes
left_poly, right_poly = polys
nonzero_x, nonzero_y = nonzeros(binary)
stacked = np.dstack((binary, binary, binary)) * 255
# Get pixel values for left lane and right lane based on fitted polynomial
plot_y = np.linspace(0, binary.shape[0] - 1, binary.shape[0])
left_fit_x = left_poly[0] * plot_y**2 + left_poly[1] * plot_y + left_poly[2]
right_fit_x = right_poly[0] * plot_y**2 + right_poly[1] * plot_y + right_poly[2]
# Show left lane in red, right lane in blue
stacked[nonzero_y[left_lane], nonzero_x[left_lane]] = [255, 0, 0]
stacked[nonzero_y[right_lane], nonzero_x[right_lane]] = [0, 0, 255]
# Plot the left and right fitted polynomial
plt.imshow(stacked)
plt.plot(left_fit_x, plot_y, color="yellow")
plt.plot(right_fit_x, plot_y, color="yellow")
plt.xlim(0, 1280)
plt.ylim(720, 0)
lanes = find_lanes(binary)
polys = fit_polys(binary, lanes)
plot_lanes(binary, lanes, polys)
In [23]:
def curvature(fit, plot_y):
xmpp = 3.7 / 700 # meters per pixel for x
ympp = 30 / 720 # meters per pixel for y
# Grab max y value, at bottom of image
y = np.max(plot_y)
# Fit the curve with values rescaled to real world physical measurements
curve = np.polyfit(ympp*plot_y, fit*xmpp, 2)
# Find the radius of curvature
rad = ((1 + (2*curve[0]*y + curve[1])**2)**1.5) / np.absolute(2*curve[0])
return rad
def center(polys):
left, right = polys
# Y value at bottom of image
y = 720
# Find value of fitted polynomial at bottom of image for left and right lane
leftx = left[0]*y**2 + left[1]*y + left[2]
rightx = right[0]*y**2 + right[1]*y + right[2]
# Return center of two values
return 0.5 * (leftx + rightx)
# All left and right polynomial values for each frame
PAST_LEFT = []
PAST_RIGHT = []
# Filtered version of PAST_ values based on threshold for changes
GOOD_LEFT = []
GOOD_RIGHT = []
# Past center values
PAST_CENTER = []
# Magnitude of differences found between frames
LEFT_DIFFS = []
RIGHT_DIFFS = []
# Diff between PAST_ and GOOD_
SKIP_COUNTS = {
"left": 0,
"right": 0
}
def rolling_avg(array, count=10):
# Grab last <count> values from <array>
data = array[-count:]
# Average those values
return sum(data) / float(len(data))
def final(img):
# Get binary overhead image
binary = pipeline(img)
# Grab y values for image
plot_y = np.linspace(0, binary.shape[0] - 1, binary.shape[0])
try:
# Grab empty image to draw road on
zeros = np.zeros_like(binary).astype(np.uint8)
color = np.dstack((zeros, zeros, zeros))
# Find the lanes
lanes = find_lanes(binary)
# Fit to polynomials
polys = fit_polys(binary, lanes)
left_poly, right_poly = polys
left_lane, right_lane = lanes
# Get values of polynomials with respect to image values
left_fit = left_poly[0]*plot_y**2 + left_poly[1]*plot_y + left_poly[2]
right_fit = right_poly[0]*plot_y**2 + right_poly[1]*plot_y + right_poly[2]
# If not the first value
if len(PAST_LEFT) != 0:
# Grab previous rolling 10 averages for left and right
prev_roll_left = rolling_avg(PAST_LEFT)
prev_roll_right = rolling_avg(PAST_RIGHT)
# Get diff between current frame and previous 10 rolling averages
left_diff = np.sum(left_fit - prev_roll_left)
right_diff = np.sum(right_fit - prev_roll_right)
# Append diffs
LEFT_DIFFS.append(left_diff)
RIGHT_DIFFS.append(right_diff)
# Only use current value if diff falls between certain range
if left_diff > -10000 and left_diff < 10000:
GOOD_LEFT.append(left_fit)
else:
SKIP_COUNTS["left"] += 1
print("Skip Left: (count: {}, value: {})".format(SKIP_COUNTS["left"], left_diff))
if right_diff > -30000 and right_diff < 30000:
GOOD_RIGHT.append(right_fit)
else:
SKIP_COUNTS["right"] += 1
print("Skip Right: (count: {}, value: {})".format(SKIP_COUNTS["right"], right_diff))
# Use rolling 10 average of the good/filtered values
roll_left = rolling_avg(GOOD_LEFT)
roll_right = rolling_avg(GOOD_RIGHT)
else:
# If first time, always append current values
GOOD_LEFT.append(left_fit)
GOOD_RIGHT.append(right_fit)
roll_left = left_fit
roll_right = right_fit
# Append to past history for next run through
PAST_LEFT.append(left_fit)
PAST_RIGHT.append(right_fit)
# Calculate distance from center
xmpp = 3.7 / 700 # meters per pixel for x
cent = ((img.shape[1] / 2.0) - center(polys)) * xmpp
PAST_CENTER.append(cent)
except Exception as e:
# If error because no pixels for a lane was found, use previous value
print("EXCEPTION in final() {}".format(e))
roll_left = rolling_avg(GOOD_LEFT)
roll_right = rolling_avg(GOOD_RIGHT)
# Transform left and right lane values into format for fillPoly
pts_left = np.array([np.transpose(np.vstack([roll_left, plot_y]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([roll_right, plot_y])))])
pts = np.hstack((pts_left, pts_right))
# Fill the road inbetween the lanes
cv2.fillPoly(color, np.int_([pts]), (0,255,0))
# Convert result back to first person view
res = fps(color)
# Add the lane drawings with some transparency onto the original image
undist = undistort(img)
res = cv2.addWeighted(undist, 1, res, 0.3, 0)
# Calculate the curvature of the left and right lanes
# Draw the curvature value on the image
curve = 0.5 * (curvature(roll_left, plot_y) + curvature(roll_right, plot_y))
cv2.putText(res, "Curvature Radius: {:.2f}km".format(curve / 1000.0), (100, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
# Draw the distance from center on the image
roll_cent = rolling_avg(PAST_CENTER)
cv2.putText(res, "Center: {:.2f}m".format(roll_cent), (100, 150), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
# Finally, return the result!
return res
img = mpimg.imread("./test_images/test1.jpg")
res = final(img)
plot_side_by_side(img, res, label2="Processed")
In [18]:
processed_filename = './out_all.mp4'
clip = VideoFileClip("./project_video.mp4")
# Process the video through our pipeline, `final()`
processed = clip.fl_image(final)
%time processed.write_videofile(processed_filename, audio=False)
HTML("""
<video width="960" height="540" controls>
<source src="{0}">
</video>
""".format(processed_filename))
Out[18]:
In [14]:
import pandas as pd
# Plot and find proper values to threshold diffs on
l = [x for x in LEFT_DIFFS if x > -10000 and x < 10000]
print(len(l))
print(len(LEFT_DIFFS))
left_series = pd.Series(l)
left_series.plot(title="Left Diffs")
Out[14]:
In [15]:
# Plot and find proper values to threshold diffs on
r = [x for x in RIGHT_DIFFS if x > -30000 and x < 30000]
print(len(r))
print(len(RIGHT_DIFFS))
right_series = pd.Series(r)
right_series.plot(title="Right Diffs")
Out[15]:
In [ ]: