def get_combined_binary(image):
binary_S = filter_hls(image)
sobel_mag = mag_thresh(image, 3, (30, 100))
# plt.imshow(sobel_mag, cmap ='gray')
# plt.show()
sobel_x = abs_sobel_thresh(image, 'x', 3, (30, 100))
print('sobel: {} ' .format(sobel_x))
# plt.imshow(sobel_x, cmap ='gray')
# plt.show()
sobel_y = abs_sobel_thresh(image, 'y', 3, (30, 100))
# plt.imshow(sobel_y, cmap ='gray')
# plt.show()
image_array = []
image_titles = []
image_array.append(image)
image_array.append(sobel_mag)
image_array.append(sobel_x)
image_array.append(sobel_y)
image_titles.append('original')
image_titles.append('sobel_mag')
image_titles.append('sobel_x')
image_titles.append('sobel_y')
# plot_figure(image_array, image_titles, 2, 2, (64, 64), 'gray')
color_binary = np.dstack((np.zeros_like(sobel_x), sobel_x, binary_S))
# plt.imshow(binary_S, cmap ='gray')
# plt.title('binary')
# plt.show()
# Combine the two binary thresholds
combined_binary = np.zeros_like(binary_S)
# combined_binary[(binary_S == 1) ] = 1
combined_binary[(sobel_y == 1) | (sobel_x == 1)] = 1
combined_binary[(binary_S > 0) | (sobel_x == 1)] = 1
#TODO REINSTATE
# plt.imshow(combined_binary, cmap ='gray')
# plt.title('test')
# plt.show()
image_array = []
image_titles = []
image_array.append(sobel_x)
image_array.append(binary_S)
image_array.append(combined_binary)
# image_array.append(cv2.cvtColor(combined_binary, cv2.COLOR_BGR2RGB))
image_titles.append('sobel_x')
image_titles.append('binary_S')
image_titles.append('combined_binary')
# plot_figure(image_array, image_titles, 1, 3, (64, 64), 'gnuplot')
# cv2.imshow("thingy",combined_binary )
# cv2.waitKey(0)
return combined_binary
# plt.show()
# Is it better to normalize?
# Choose a Sobel kernel size
ksize = 9 # Choose a larger odd number to smooth gradient measurements
# Apply each of the thresholding functions
gradx = util.abs_sobel_thresh(hls_binary, orient='x', sobel_kernel=ksize, thresh=(40, 100))
# plt.title('gradx')
# plt.imshow(gradx,cmap='gray')
# plt.show()
grady = util.abs_sobel_thresh(hls_binary, orient='y', sobel_kernel=ksize, thresh=(40, 100))
# plt.title('grady')
# plt.imshow(grady,cmap='gray')
# plt.show()
mag_binary = util.mag_thresh(hls_binary, sobel_kernel=ksize, mag_thresh=(30, 100))
# plt.title('magnitude')
# plt.imshow(mag_binary,cmap='gray')
# plt.show()
dir_binary = util.dir_threshold(hls_binary, sobel_kernel=ksize, thresh=(0.7, 1.3))
# plt.title('direction')
# plt.imshow(dir_binary,cmap='gray')
# plt.show()
combined = np.zeros_like(hls_binary)
combined[(((gradx == 1) & (grady == 1)) | ((mag_binary == 1) & (dir_binary == 1))) | ((R>128+48) & (G>128+48))] = 1
# plt.title('COMBINED')
# plt.imshow(combined,cmap='gray')
# plt.show()
# plt.imsave('combined_gray_scale.png',combined) # To check src points manually.
cv2.imshow('img', raw_img)
cv2.waitKey(0)
### 1. Undistort image
print("Undistorted image")
image = cv2.undistort(raw_img, mtx, dist, None, mtx)
cv2.imshow('img', image)
cv2.waitKey(0)
### 2. Detect corners using different thresholds:
print("Binary corner image")
H, L, S = f.convert_hls(image)
gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
gradx = f.abs_sobel_thresh(gray, orient='x', abs_thresh=abs_t)
mag_binary = f.mag_thresh(S, sobel_kernel=kernel, mag_thresh=mag_t)
sat_binary = f.saturation_thresh(image, hls_thresh=sat_t)
# 3. Combine different channels
combined = np.zeros_like(gradx)
combined[(gradx == 1) | (sat_binary == 1) & (mag_binary == 1)] = 1
cv2.imshow('img', combined * 255)
cv2.waitKey(0)
### 4. Mask region of interest
print("Mask region of interest")
masked = f.region_of_interest(
combined,
np.array([[(s1[0], s1[1]), (s2[0], s2[1]), (s3[0], s3[1]),
def process_image(image_org):
global window_pos
global frame_no
global left_fit_detected
global right_fit_detected
global last_curve_left
global last_curve_right
global last_fit_left
global last_fit_right
# plt.imsave('image{:}.png'.format(frame_no), image_org)
# frame_no = frame_no + 1
# return image_org
try:
# picked up image processing from find_lane.py after it turned out working with still image.
image_undistort = cv2.undistort(image_org, mtx, dist, None, mtx)
image_normalize = (image_undistort - np.mean(image_undistort)
) / np.std(image_undistort) * 32 + 128
R = image_normalize[:, :, 0]
G = image_normalize[:, :, 1]
hls_binary = util.hls_select(image_undistort, thresh=(90, 255))
# plt.title('S channel with thresh')
# plt.imshow(hls_binary, cmap='gray')
# plt.show()
gradx = util.abs_sobel_thresh(hls_binary,
orient='x',
sobel_kernel=ksize,
thresh=(40, 100))
grady = util.abs_sobel_thresh(hls_binary,
orient='y',
sobel_kernel=ksize,
thresh=(40, 100))
grad = np.zeros_like(hls_binary)
grad[((gradx == 1) & (grady == 1))] = 1
mag_binary = util.mag_thresh(hls_binary,
sobel_kernel=ksize,
mag_thresh=(30, 100))
dir_binary = util.dir_threshold(hls_binary,
sobel_kernel=ksize,
thresh=(0, 0))
combined = np.zeros_like(hls_binary)
combined[(((gradx == 1) & (grady == 1)) | ((mag_binary == 1) &
(dir_binary == 1))) |
((R > 128 + 48) & (G > 128 + 48))] = 1
bird_view = util.warper(combined, src_points, dst_points)
if (len(history_left_x) > 0):
if (len(history_left_x) < ref_frames):
end_pos = -1 - len(history_left_x)
else:
end_pos = -1 - ref_frames
leftx_with_hist = np.hstack(
history_left_x[-1:end_pos:-1]).flatten()
lefty_with_hist = np.hstack(
history_left_y[-1:end_pos:-1]).flatten()
rightx_with_hist = np.hstack(
history_right_x[-1:end_pos:-1]).flatten()
righty_with_hist = np.hstack(
history_right_y[-1:end_pos:-1]).flatten()
else:
leftx_with_hist = np.empty([
0,
])
lefty_with_hist = np.empty([
0,
])
rightx_with_hist = np.empty([
0,
])
righty_with_hist = np.empty([
0,
])
if (left_fit_detected == False or right_fit_detected == False):
leftx, lefty, rightx, righty, window_pos_ret = util.find_lane_pixels_with_history(
bird_view, nwindows, margin, minpix, window_pos,
leftx_with_hist, lefty_with_hist, rightx_with_hist,
righty_with_hist)
window_pos = window_pos_ret
else:
leftx, lefty, rightx, righty = util.search_around_poly(
bird_view, last_fit_left, last_fit_right, margin)
window_pos = [(0, 0, 0, False, False)] #reset
# save for future reuse
history_left_x.append([leftx])
history_left_y.append([lefty])
history_right_x.append([rightx])
history_right_y.append([righty])
# color_fit_lines = np.zeros( bird_view.shape + ( 3, ), dtype=np.int32 )
# color_fit_lines[lefty,leftx] = [255, 0, 0]
# color_fit_lines[righty,rightx] = [0, 0, 255 ]
if (len(history_left_x) > 1):
if (len(history_left_x) < ref_frames):
end_pos = -1 - len(history_left_x)
else:
end_pos = -1 - ref_frames
leftx_with_hist = np.hstack(
history_left_x[-1:end_pos:-1]).flatten()
lefty_with_hist = np.hstack(
history_left_y[-1:end_pos:-1]).flatten()
rightx_with_hist = np.hstack(
history_right_x[-1:end_pos:-1]).flatten()
righty_with_hist = np.hstack(
history_right_y[-1:end_pos:-1]).flatten()
else:
leftx_with_hist = leftx
lefty_with_hist = lefty
rightx_with_hist = rightx
righty_with_hist = righty
l_lane_detect_count = 0
r_lane_detect_count = 0
for (l, r, h, l_found, r_found) in window_pos:
if (l_found):
l_lane_detect_count = l_lane_detect_count + 1
if (r_found):
r_lane_detect_count = r_lane_detect_count + 1
pic_height = bird_view.shape[0]
left_fitx, right_fitx, left_fit, right_fit = util.fit_polynomial(
leftx_with_hist, lefty_with_hist, rightx_with_hist,
righty_with_hist, pic_height)
if (len(window_pos) > 2):
if (l_lane_detect_count <
1): # do not trust polynomial fit. use previous one
left_fitx = last_curve_left
if (r_lane_detect_count <
1): # do not trust polynomial fit. use previous one
right_fitx = last_curve_right
if (l_lane_detect_count > nwindows * 2 / 3): # trust polynomial
left_fit_detected = True
last_fit_left = left_fit
else:
left_fit_detected = False
if (r_lane_detect_count > nwindows * 2 / 3): # trust polynomial
right_fit_detected = True
last_fit_right = right_fit
else:
right_fit_detected = False
last_curve_left = left_fitx
last_curve_right = right_fitx
ploty = np.linspace(0, pic_height - 1, pic_height)
color_with_lane_region = np.zeros(bird_view.shape + (3, ),
dtype=np.uint8)
poly = np.vstack((np.array([left_fitx, ploty], dtype=np.int32).T,
np.array([right_fitx, ploty],
dtype=np.int32).T[::-1]))
color_with_lane_region = cv2.fillPoly(color_with_lane_region, [poly],
color=[0, 32, 0])
color_with_lane_region[lefty_with_hist, leftx_with_hist] = [255, 0, 0]
color_with_lane_region[righty_with_hist,
rightx_with_hist] = [0, 0, 255]
window_height = np.int(color_with_lane_region.shape[0] // nwindows)
l_lane_detect_count = 0
r_lane_detect_count = 0
for (l, r, h, l_found, r_found) in window_pos:
if (l_found):
cv2.rectangle(color_with_lane_region,
(l - margin, h - window_height), (l + margin, h),
(255, 0, 0), 3)
if (r_found):
cv2.rectangle(color_with_lane_region,
(r - margin, h - window_height), (r + margin, h),
(0, 0, 255), 3)
cv2.imshow('line', color_with_lane_region)
cv2.waitKey(1)
left_fit_in = left_fitx[::-1] # Reverse to match top-to-bottom in y
right_fit_in = right_fitx[::-1] # Reverse to match top-to-bottom in y
left_fit_cr = np.polyfit(ploty * ym_per_pix, left_fit_in * xm_per_pix,
2)
right_fit_cr = np.polyfit(ploty * ym_per_pix,
right_fit_in * xm_per_pix, 2)
left_curverad, right_curverad = util.measure_curvature_real(
left_fit_cr, right_fit_cr, ploty, xm_per_pix, ym_per_pix)
curverad = np.mean([left_curverad, right_curverad])
center = left_fit_in[0] + (right_fit_in[0] - left_fit_in[0]) / 2
camera_center_pix = bird_view.shape[1] / 2
if (center == camera_center_pix):
car_position_string = "CENTER"
elif (center > camera_center_pix):
diff = (center - camera_center_pix) * xm_per_pix
car_position_string = "Vehcle is {:.2f}m left of center".format(
diff)
else:
diff = (camera_center_pix - center) * xm_per_pix
car_position_string = "Vehcle is {:.2f}m right of center".format(
diff)
camera_view = util.warper(color_with_lane_region, dst_points,
src_points)
img_overlayed = util.weighted_img(camera_view.astype(np.uint8),
image_undistort)
font = cv2.FONT_HERSHEY_SIMPLEX
x_start = int(img_overlayed.shape[1] / 100)
y_start = int(img_overlayed.shape[0] / 10)
y_inc = int(img_overlayed.shape[0] / 20)
font_size = 1.0
cv2.putText(img_overlayed, 'R={:>8d} meter'.format(int(curverad)),
(x_start, y_start), font, font_size, (255, 255, 255), 2,
cv2.LINE_AA)
cv2.putText(img_overlayed, car_position_string,
(x_start, y_start + y_inc * 1), font, font_size,
(255, 255, 255), 2, cv2.LINE_AA)
except TypeError:
traceback.print_exc()
img_overlayed = image_org
cv2.imshow('debug', img_overlayed)
cv2.waitKey(1)
frame_no = frame_no + 1
return img_overlayed
def process_single_frame(self, raw_img):
if not calibrate_done:
calibrate_camera(cal_folder, pickle_file)
mtx, dist = f.load_calibrate_parameters(cal_folder, pickle_file)
### 1. Undistort image
image = cv2.undistort(raw_img, mtx, dist, None, mtx)
### 2. Detect corners using different thresholds:
H, L, S = f.convert_hls(image)
B,G,R = image[:,:,0], image[:,:,1], image[:,:,2]
#gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
gradx = f.abs_sobel_thresh(R, orient='x', abs_thresh=abs_t)
mag_binary = f.mag_thresh(S, sobel_kernel=kernel, mag_thresh = mag_t)
sat_binary = f.saturation_thresh(image, hls_thresh=sat_t)
int_r = f.intensity_thresh(R, int_t)
grx = (gradx == 1)
sat = (sat_binary == 1)
mag = (mag_binary == 1)
itr = (int_r == 1)
### 3. Combine different channels
combined = np.zeros_like(gradx)
combined[grx | sat & mag] = 1
### 4. Mask region of interest
masked = f.region_of_interest(combined, np.array([[(s1[0], s1[1]), (s2[0], s2[1]), (s3[0], s3[1]), (s4[0], s4[1])]], dtype=np.int32))
### 5. Perspective transform
# Perspective transform parameters
src = np.float32([s1, s2, s3, s4])
dst = np.float32([d1, d2, d3, d4])
M = cv2.getPerspectiveTransform(src,dst)
Minv = cv2.getPerspectiveTransform(dst, src)
warped = cv2.warpPerspective(masked, M, (combined.shape[1]+offsetx, combined.shape[0]+offsety), flags=cv2.INTER_LINEAR)
### 6. Fit polynomial
poly_img, left_fit, right_fit, left_fitx, right_fitx, ploty = f.fit_polynomial(warped, visualization = True)
'''
poly_img, left_fit, right_fit, left_fitx, right_fitx, ploty = f.search_around_poly(warped, left_fit, right_fit, visualization = True)
cv2.imshow('img', poly_img)
cv2.waitKey(0)
'''
### 7. Print free lane space
Y_range = range(0, warped.shape[0]-1)
free_area = f.color_free_area(poly_img, left_fit, right_fit, Y_range)
### 8. Print unwraped result
unwarped = cv2.warpPerspective(free_area, Minv, (combined.shape[1], combined.shape[0]), flags=cv2.INTER_LINEAR)
### 9. Final result
final = cv2.addWeighted(image, 1.0, unwarped, 0.8, 0)
### Fitting polynomial to find radius in meters
y_eval = np.max(ploty)
left_fit_cr = np.polyfit(ploty*ym_per_pix, left_fitx*xm_per_pix, 2)
right_fit_cr = np.polyfit(ploty*ym_per_pix, right_fitx*xm_per_pix, 2)
left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
right_curverad = ((1 + (2*right_fit_cr[0]*y_eval*ym_per_pix + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])
#print("Left curvature: %f m" % left_curverad )
#print("Right curvature: %f m" % right_curverad )
average_curvature = np.average([left_curverad, right_curverad])
### Calculate vehicle deviation from center
x_left_pixels = left_fit[0]*y_eval**2 + left_fit[1]*y_eval + left_fit[2]
x_right_pixels = right_fit[0]*y_eval**2 + right_fit[1]*y_eval + right_fit[2]
average_pixels = int(np.average([x_left_pixels, x_right_pixels]))
center_pixels = image.shape[1]//2
diff_pixels = center_pixels - average_pixels
# Display result
text1 = "Curvature: %.2f (m)" % average_curvature
text2 = "Vehicle is %.2f (m) to the %s ." % (abs(diff_pixels * xm_per_pix), ("right" if (diff_pixels > 0) else "left") )
cv2.putText(final,text1,
(bottomLeftCornerOfText[0],bottomLeftCornerOfText[1]),
font,
fontScale,
fontColor,
lineType)
cv2.putText(final,text2,
(bottomLeftCornerOfText[0],bottomLeftCornerOfText[1]+30),
font,
fontScale,
fontColor,
lineType)
return final