def main():
if (len(sys.argv) > 1) and isinstance(sys.argv[1], str):
filename = sys.argv[1]
else:
filename = 'challenge_video.mp4'
print('Processing file ' + filename)
white_output = 'processed_videos/' + filename
clip1 = VideoFileClip('source_videos/' + filename) #.subclip(0,5)
# calculate matrices for perspective transformation
target_left_x = 300
target_right_x = 1002
target_top_y = 0
target_bottom_y = 690
src_points = np.float32([[283, 664], [548, 480], [736, 480], [1019, 664]])
dst_points = np.float32([[target_left_x, target_bottom_y],
[target_left_x, target_top_y],
[target_right_x, target_top_y],
[target_right_x, target_bottom_y]])
# transformation to bird's eye view
M = cv2.getPerspectiveTransform(src_points, dst_points)
# transformation back to normal view
Mi = cv2.getPerspectiveTransform(dst_points, src_points)
# calculate or load camera calibration
calCam = CalibrateCamera.load()
if calCam == None:
images = glob.glob('camera_cal/calibration*.jpg')
calCam = CalibrateCamera()
calCam.findCorners(images, (9, 6))
calCam.calibrateCamera()
calCam.write()
# class which will process the images, initialize with image size and
# transformation matrices
ld = ProcessImage()
ld.fit((720, 1280), M, Mi, calCam=calCam)
white_clip = clip1.fl_image(ld.process_image) # color images
white_clip.write_videofile(white_output, audio=False)
def fit(self, shape, M, MInverse, roi=None, calCam=None):
"""
First call before processing images
"""
self.shape = shape
# region of interest
if roi != None:
self.roi = roi
else:
self.roi = {
'bottom_y': shape[0],
'bottom_x_left': int(shape[1] * 0.05),
'bottom_x_right': int(shape[1] * 0.95),
'top_y': int(shape[0] * 0.6),
'top_x_left': int(shape[1] * 0.45),
'top_x_right': int(shape[1] * 0.55),
}
self.roi.update({
'top_center':
int((self.roi['top_x_left'] + self.roi['top_x_right']) / 2)
})
if calCam is not None:
self.calCam = calCam
else:
self.calCam = CalibrateCamera.load()
if self.calCam is None:
images = glob.glob('camera_cal/calibration*.jpg')
self.calCam = CalibrateCamera()
self.calCam.findCorners(images, (9, 6))
self.calCam.calibrateCamera()
self.calCam.write()
self.laneFit = LaneFit()
self.M = M
self.MInverse = MInverse
self.frame = 0
self.update = False
def main(args):
#camera calibration
calCam = CalibrateCamera.load()
# if no precalculated data is available start calculation
if calCam == None:
images = glob.glob('data/camera_cal/*.jpg')
calCam = CalibrateCamera()
calCam.findCorners(images, (9, 6))
calCam.calibrateCamera()
calCam.write()
# create subscriber class
ims = image_subscriber(calCam)
rospy.init_node('show_robot_camera')
rate = rospy.Rate(30)
while not rospy.is_shutdown():
ims.show_image()
rate.sleep()
cv2.destroyAllWindows()
class ProcessImageLane():
def __init__(self):
self.shape = None
self.roi = None
self.calCam = None
self.laneFit = None
self.image_cnt = 0
self.DEBUG_IMAGE = True
self.DEBUG_IMAGE_FOLDER = 'challenge_debug'
self.DEBUG_VIDEO = False
self.segmentation = 1
def fit(self, shape, M, MInverse, roi=None, calCam=None):
"""
First call before processing images
"""
self.shape = shape
# region of interest
if roi != None:
self.roi = roi
else:
self.roi = {
'bottom_y': shape[0],
'bottom_x_left': int(shape[1] * 0.05),
'bottom_x_right': int(shape[1] * 0.95),
'top_y': int(shape[0] * 0.6),
'top_x_left': int(shape[1] * 0.45),
'top_x_right': int(shape[1] * 0.55),
}
self.roi.update({
'top_center':
int((self.roi['top_x_left'] + self.roi['top_x_right']) / 2)
})
if calCam is not None:
self.calCam = calCam
else:
self.calCam = CalibrateCamera.load()
if self.calCam is None:
images = glob.glob('camera_cal/calibration*.jpg')
self.calCam = CalibrateCamera()
self.calCam.findCorners(images, (9, 6))
self.calCam.calibrateCamera()
self.calCam.write()
self.laneFit = LaneFit()
self.M = M
self.MInverse = MInverse
self.frame = 0
self.update = False
def writeInfo(self, img, laneparams):
"""
Writes information to the image
"""
# empty image
box_img = np.zeros_like(img).astype(np.uint8)
# draw rectangle and arrow for position deviation
box_img = cv2.rectangle(box_img, (10, 10), (int(1280 / 2 - 10), 150),
(0, 0, 100),
thickness=cv2.FILLED)
box_img = cv2.rectangle(box_img, (int(1280 / 2 - 10), 10),
(1280 - 10, 150), (0, 0, 100),
thickness=cv2.FILLED)
box_img = cv2.arrowedLine(
box_img, (500, 60),
(int(500 + laneparams['middle_phys'] * 200), 60), (255, 0, 0), 5)
img = cv2.addWeighted(img, 1.0, box_img, 1.0, 0.)
font = cv2.FONT_HERSHEY_SIMPLEX
pos_str = 'Left curve: {:06.0f}'.format(laneparams['left_curverad'])
pos_str2 = 'Right curve: {:06.0f}'.format(laneparams['right_curverad'])
pos_str3 = 'Middle: {:.2f}m'.format(laneparams['middle_phys'])
frame_str = 'Frame: {}'.format(self.frame)
left_pos = 40
top_pos = 40
delta_height = 30
# write text to image
cv2.putText(img, pos_str, (left_pos, top_pos), font, 0.8,
(255, 255, 255), 1, cv2.LINE_AA)
cv2.putText(img, pos_str2, (left_pos, top_pos + delta_height), font,
0.8, (255, 255, 255), 1, cv2.LINE_AA)
cv2.putText(img, pos_str3, (400, top_pos), font, 0.8, (255, 255, 255),
1, cv2.LINE_AA)
cv2.putText(img, frame_str, (left_pos, top_pos + 3 * delta_height),
font, 0.8, (255, 255, 255), 1, cv2.LINE_AA)
return img
def process_image(self, img_orig):
"""
Processes an image
"""
t0 = time()
img = self.calCam.undistort(img_orig)
if self.DEBUG_IMAGE:
img_savename = 'image{0:04d}.jpg'.format(self.image_cnt)
cv2.imwrite(self.DEBUG_IMAGE_FOLDER + '/original/' + img_savename,
cv2.cvtColor(img_orig, cv2.COLOR_BGR2RGB))
cv2.imwrite(self.DEBUG_IMAGE_FOLDER + '/undist/' + img_savename,
cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
img_undist = img
ksize = 9
# gray conversion
gray = img2gray(img)
# Apply each of the thresholding functions
if self.segmentation == 0:
# magnitude and direction of edges, color
mag_binary = mag_thresh(gray,
sobel_kernel=ksize,
mag_thresh=(20, 255))
dir_binary = dir_threshold(gray,
sobel_kernel=ksize,
thresh=(np.pi / 4 * 0.9,
np.pi / 4 * 1.5))
color_seg = color_segmentation(img)
seg_img_raw = color_seg & mag_binary & dir_binary
else:
# color segmentation and canny edge detection
color_seg = color_segmentation(img)
kernel_size = 5
blur_gray = cv2.GaussianBlur(gray, (kernel_size, kernel_size), 0)
canny = cv2.Canny(blur_gray, 40, 80).astype(np.uint8) * 255
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
# dilate canny edges which are very sharp
canny = cv2.dilate(canny, kernel, iterations=2)
# segmented image with color and canny
seg_img_raw = (color_seg & canny)
if self.DEBUG_IMAGE:
cv2.imwrite(
self.DEBUG_IMAGE_FOLDER + '/seg_color/' + img_savename,
color_seg.astype(np.uint8) * 255)
cv2.imwrite(
self.DEBUG_IMAGE_FOLDER + '/seg_canny/' + img_savename,
canny.astype(np.uint8) * 255)
cv2.imwrite(
self.DEBUG_IMAGE_FOLDER + '/seg_comb/' + img_savename,
seg_img_raw.astype(np.uint8) * 255)
seg_img = seg_img_raw.astype(np.uint8) * 255
seg_img[690:, :] = 0
# mask image
# region of interest not used
#seg_img, vertices = mask_region_of_interest(seg_img, self.roi)
seg_img = np.dstack((seg_img, seg_img, seg_img))
#visualization = np.dstack((np.zeros(dir_binary.shape), (dir_binary & mag_binary), color_seg)).astype(np.uint8) * 255
# warp to birds eye perspective
seg_img_warped = cv2.warpPerspective(
seg_img,
self.M, (seg_img.shape[1], seg_img.shape[0]),
flags=cv2.INTER_LINEAR)
# thresholding for interpolated pixels
seg_img_warped = (seg_img_warped > 100).astype(np.uint8) * 255
if self.DEBUG_IMAGE:
cv2.imwrite(
self.DEBUG_IMAGE_FOLDER + '/seg_warped/' + img_savename,
seg_img_warped)
#undist_img_warped = cv2.warpPerspective(img_undist, self.M, (img_undist.shape[1], img_undist.shape[0]), flags=cv2.INTER_LINEAR)
#cv2.imwrite(self.DEBUG_IMAGE_FOLDER + '/undist_warped/'+img_savename, undist_img_warped)
# do LaneFit algorithm on image
laneparams = self.laneFit.procVideoImg(seg_img_warped,
margin=60,
numwin=20)
lane_img = laneparams['img']
if self.DEBUG_IMAGE:
comb = cv2.addWeighted(seg_img_warped, 0.5, lane_img, 0.8, 0)
cv2.imwrite(self.DEBUG_IMAGE_FOLDER + '/combined/' + img_savename,
cv2.cvtColor(comb, cv2.COLOR_BGR2RGB))
lane_img_unwarped = cv2.warpPerspective(
lane_img,
self.MInverse, (lane_img.shape[1], lane_img.shape[0]),
flags=cv2.INTER_LINEAR)
# combine original image with detection
img = cv2.addWeighted(img, 1, lane_img_unwarped, 0.7, 0)
# time measurement
t1 = time()
logging.info('process_image: runtime = ' + str(t1 - t0))
# debug video with different segmentations used in this algorithm
if self.DEBUG_VIDEO:
img = np.dstack(
(np.zeros_like(seg_img_raw), canny, color_seg)).astype(
np.uint8) * 255
img = cv2.addWeighted(img, 1, lane_img_unwarped, 0.9, 0)
if self.DEBUG_IMAGE:
cv2.imwrite(self.DEBUG_IMAGE_FOLDER + '/result/' + img_savename,
cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
self.image_cnt += 1 # for number of debug image
undist_img_warped = cv2.warpPerspective(
img_undist,
self.M, (img_undist.shape[1], img_undist.shape[0]),
flags=cv2.INTER_LINEAR)
# add smaller images of the segmented and warped image
y_offset = 0
output_size = (720, 1280, 3)
ret_img = np.zeros(output_size).astype(np.uint8)
warped_smaller = cv2.resize(undist_img_warped, (0, 0),
fx=0.15,
fy=0.15)
warped_semented_smaller = cv2.resize(comb, (0, 0), fx=0.15, fy=0.15)
warped_combined_smaller = cv2.addWeighted(warped_smaller, 1,
warped_semented_smaller, 0.7,
0)
color_smaller = cv2.resize(
color_seg.astype(np.uint8), (0, 0), fx=0.15, fy=0.15).reshape(
(108, 192, 1)) * 255
canny_smaller = cv2.resize(
canny.astype(np.uint8), (0, 0), fx=0.15, fy=0.15).reshape(
(108, 192, 1)) * 255
combined_smaller = cv2.resize(
(color_seg & canny).astype(np.uint8),
(0, 0), fx=0.15, fy=0.15).reshape((108, 192, 1)) * 255
stacked_smaller = np.dstack(
(combined_smaller, canny_smaller, color_smaller)).astype(np.uint8)
ret_img[y_offset:img.shape[0] + y_offset, :img.shape[1], :] = img
# write information to image
ret_img = self.writeInfo(ret_img, laneparams)
offset_small = 26
offset_x_small = 684
ret_img[offset_small:offset_small + 108,
offset_x_small:offset_x_small + 192, :] = stacked_smaller
ret_img[offset_small:offset_small + 108, offset_x_small +
200:offset_x_small + 392, :] = warped_combined_smaller
self.frame += 1
return ret_img
def main():
calCam = CalibrateCamera.load()
if calCam == None:
images = glob.glob('camera_cal/calibration*.jpg')
calCam = CalibrateCamera()
calCam.findCorners(images, (9, 6))
calCam.calibrateCamera()
calCam.write()
print(calCam.mtx)
print(calCam.dist)
# Read in an image
img_orig = mpimg.imread('test_images/straight_lines2.jpg')
#img_orig = mpimg.imread('test_images/test6.jpg')
#img_orig = (mpimg.imread('test_images/shadow_05.png') * 255).astype(np.uint8)
#img_orig = (mpimg.imread('test_images/challenge_02.png') * 255).astype(np.uint8)
#img_orig = mpimg.imread('camera_cal/calibration1.jpg')
img = calCam.undistort(img_orig)
# define region of interest
roi = {
'bottom_y': img.shape[0],
'bottom_x_left': int(img.shape[1] * 0.05),
'bottom_x_right': int(img.shape[1] * 0.95),
'top_y': int(img.shape[0] * 0.6),
'top_x_left': int(img.shape[1] * 0.45),
'top_x_right': int(img.shape[1] * 0.55),
}
roi.update(
{'top_center': int((roi['top_x_left'] + roi['top_x_right']) / 2)})
horizon = 425
if False:
original_bottom_left_x = 283
target_left_x = 300
target_right_x = 1002
target_top_y = 0
target_bottom_y = 685
src_points = np.float32([[283, 664], [552, 480], [736, 480],
[1015, 664]])
dst_points = np.float32([[target_left_x, target_bottom_y],
[target_left_x, target_top_y],
[target_right_x, target_top_y],
[target_right_x, target_bottom_y]])
else:
target_left_x = 300
target_right_x = 1002
target_top_y = 0
target_bottom_y = 690
src_points = np.float32([[283, 664], [548, 480], [736, 480],
[1019, 664]])
dst_points = np.float32([[target_left_x, target_bottom_y],
[target_left_x, target_top_y],
[target_right_x, target_top_y],
[target_right_x, target_bottom_y]])
M = cv2.getPerspectiveTransform(src_points, dst_points)
Mi = cv2.getPerspectiveTransform(dst_points, src_points)
ksize = 15 # Choose a larger odd number to smooth gradient measurements
#hls = cv2.cvtColor(img[roi['top_y']:,:,:], cv2.COLOR_RGB2HLS)
hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(32, 32))
hls[:, :, 1] = clahe.apply(hls[:, :, 1])
#hls[:,:,1] = cv2.equalizeHist(hls[:,:,1])
claheimg = cv2.cvtColor(hls, cv2.COLOR_HLS2RGB)
gray = img2gray(img)
# Apply each of the thresholding functions
gradx = abs_sobel_thresh(gray,
orient='x',
sobel_kernel=ksize,
thresh=(20, 255))
grady = abs_sobel_thresh(gray,
orient='y',
sobel_kernel=ksize,
thresh=(30, 255))
mag_binary = mag_thresh(gray, sobel_kernel=ksize, mag_thresh=(30, 255))
color_mag = mag_thresh(gray, sobel_kernel=ksize, mag_thresh=(5, 255))
dir_binary = dir_threshold(
gray, sobel_kernel=ksize,
thresh=(1.0, 1.3)) #thresh=(np.pi/4*1.0, np.pi/4*1.2))
kernel_size = 5
blur_gray = cv2.GaussianBlur(gray, (kernel_size, kernel_size), 0)
#canny = cv2.Canny(blur_gray, 120, 200)
canny = cv2.Canny(blur_gray, 40, 80)
canny[670:, :] = 0
canny = np.dstack((canny, canny, canny))
#print(src_points.reshape((-1,1,2)).astype(np.int32))
#pts = np.array([[10,5],[20,30],[70,20],[50,10]], np.int32)
#src_points = pts.reshape((-1,1,2))
#print(src_points)
canny = cv2.polylines(canny,
[src_points.reshape((-1, 1, 2)).astype(np.int32)],
True, (0, 255, 255),
thickness=1)
canny_warped = cv2.warpPerspective(canny,
M, (canny.shape[1], canny.shape[0]),
flags=cv2.INTER_LINEAR)
#canny_warped = (canny_warped > 120).astype(np.uint8) * 255
#kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(5,5))
#canny_warped = cv2.dilate(canny_warped, kernel, iterations=1)
#canny_warped = cv2.erode(canny_warped, kernel, iterations=1)
plt.imshow(canny)
plt.show()
return
color_seg = color_segmentation(img,
l_thresh=[30, 255],
s_thresh=[160, 255])
seg_img_raw = (
(color_seg & color_mag & dir_binary) | (dir_binary & mag_binary)
) #(color_seg | (dir_binary & mag_binary)).astype(np.uint8) * 255
# mask image
seg_img, vertices = mask_region_of_interest(
seg_img_raw.astype(np.uint8) * 255, roi)
seg_img_roi = seg_img
#visualization = np.dstack((seg_img_raw, (dir_binary & mag_binary), color_seg)).astype(np.uint8) * 255
visualization = np.dstack(
(seg_img_raw, color_seg,
(dir_binary & mag_binary))).astype(np.uint8) * 255
seg_img = np.dstack((seg_img, seg_img, seg_img))
seg_img_warped = cv2.warpPerspective(seg_img,
M,
(seg_img.shape[1], seg_img.shape[0]),
flags=cv2.INTER_LINEAR)
histogram = np.sum(seg_img_warped[gradx.shape[0] // 2:, :-2, 0], axis=0)
midpoint = np.int(histogram.shape[0] / 2)
print('Midpoint = ' + str(midpoint))
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
print('Bases = ' + str((leftx_base, rightx_base)))
laneFit = LaneFit()
left_fit, right_fit, lane_img, _, _, _ = laneFit.fitLanes(seg_img_warped,
leftx_base,
rightx_base,
margin=60)
lane_img_unwarped = cv2.warpPerspective(
lane_img,
Mi, (lane_img.shape[1], lane_img.shape[0]),
flags=cv2.INTER_LINEAR)
# Plot the result
gs = grd.GridSpec(3,
2,
height_ratios=[10, 10, 10],
width_ratios=[1, 1],
wspace=0.1)
ax = plt.subplot(gs[0, 0])
ax.imshow(img_orig)
ax.set_title('Original Image', fontsize=10)
ax = plt.subplot(gs[0, 1])
ax.imshow(img)
ax.set_title('Undistorted Image', fontsize=10)
if False:
ax = plt.subplot(gs[1, 0])
ploty = np.linspace(0, lane_img.shape[0] - 1, lane_img.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]
ax.imshow(lane_img)
ax.plot(left_fitx, ploty, color='yellow')
ax.plot(right_fitx, ploty, color='yellow')
ax.set_title('Lane fit', fontsize=10)
else:
ax = plt.subplot(gs[1, 0])
#ax.imshow(claheimg)
ax.imshow(canny, cmap='gray')
ax = plt.subplot(gs[1, 1])
ax.imshow(blur_gray, cmap='gray')
ax.set_title('Combined', fontsize=10)
ax = plt.subplot(gs[2, 0])
ax.imshow(visualization)
ax.set_title('Visualization', fontsize=10)
ax = plt.subplot(gs[2, 1])
#ax.plot(histogram)
ax.imshow(lane_img_unwarped)
ax.set_title('Histogram', fontsize=10)
plt.show()