def annotate_image(img_in):
"""
Annotate the input image with lane line markings
Returns annotated image
"""
global mtx, dist, left_line, right_line, detected
global left_lane_inds, right_lane_inds
# threshold, perspective transform
img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(img_in)
binary_warped, binary_unwarped, m, m_inv = perspective_transform(img)
# Perform polynomial fit
if not detected:
# Slow line fit
ret = line_fit(binary_warped)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Get moving average of line fit coefficients
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
detected = True # slow line fit always detects the line
else: # implies detected == True
# Fast line fit
left_fit = left_line.get_fit()
right_fit = right_line.get_fit()
ret = tune_fit(binary_warped, left_fit, right_fit)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Only make updates if we detected lines in current frame
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
else:
detected = False
vehicle_offset = calc_vehicle_offset(img_in, left_fit, right_fit)
result = final_viz(img_in, left_fit, right_fit, m_inv, vehicle_offset)
return result
def callback(self, data):
global count
try:
img = self.bridge.imgmsg_to_cv2(data, desired_encoding="bgr8")
if img.dtype == 'float32':
img = np.array(img) * 255
img = np.uint8(img)
img = cv2.blur(img, (5, 5))
img, _, _, _, _ = combined_thresh(img)
#_, _, img = combined_thresh_canny(img)
#img, _, _, _ = perspective_transform(img)
cv2.imshow("Image window", img.astype(np.float32))
count += 1
k = cv2.waitKey(1)
if k == 113: #q
cv2.imwrite("saves/" + str(count) + "_igvcw.png", img)
if k == 27: #esc
cv2.destroyAllWindows()
try:
ret = line_fit(img)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
bottom_y = img.shape[0] - 1
bottom_x_left = left_fit[0] * (
bottom_y**2) + left_fit[1] * bottom_y + left_fit[2]
bottom_x_right = right_fit[0] * (
bottom_y**2) + right_fit[1] * bottom_y + right_fit[2]
vehicle_offset = img.shape[1] / 2 - (bottom_x_left +
bottom_x_right) / 2
# if 350 < bottom_x_right - bottom_x_left < 600 :
# self.pub.publish(0.0)
# return 0
xm_per_pix = 3.7 / 680 # meters per pixel in x dimension
vehicle_offset = vehicle_offset * xm_per_pix
self.pub.publish(vehicle_offset) # cross track error
except TypeError:
print("No lanes found.")
self.pub.publish(0.0)
#label_str = 'Vehicle offset from lane center: %.3f m' % self.vehicle_offset
#img = cv2.putText(img, label_str, (30,70), 0, 1, (255,0,0), 2, cv2.LINE_AA)
except CvBridgeError as e:
print(e)
def pipeline(img, mtx, dist):
undist = cv2.undistort(img, mtx, dist, None, mtx)
# plt.imshow(undist)
# plt.show()
edges = find_edges(undist)
# plt.imshow(edges, cmap="gray")
# plt.show()
warped, M = bv_transform(edges)
# plt.imshow(warped, cmap="gray")
# plt.show()
left_fit, right_fit, left_curverad, right_curverad, car_position = line_fit(warped)
result = draw_lines(undist, warped, left_fit, right_fit, M)
draw_curv_and_dist(result, left_curverad, right_curverad, car_position)
# out_img = (np.dstack((edges, edges, edges)) * 255)
# out_img = out_img.astype(np.uint8)
# return out_img
return result
#rows, cols, ch = test_image.shape
cv2.warpPerspective(img_gray, M, (200, 320), fushi, cv2.INTER_LINEAR)
#fushi = line_fit(fushi)
kernel = np.ones((7, 7), np.float32) / 25
dst = cv2.filter2D(fushi, -1, kernel)
dst = cv2.Canny(dst, 60, 180)
#dst = cv2.dilate(dst,erode_k)
dst = cv2.bitwise_and(dst, dst, mask=mask)
dst = cv2.dilate(dst, erode_k)
dst = cv2.dilate(dst, erode_k)
lines = line_fit(dst)
#dst = cv2.warpPerspective(dst, M, (200,320))
#lines = cv2.HoughLines(dst,1, np.pi/180, 1)
print idx
#print lines
try:
line = lines[:, 0, :]
for rho, theta in line[:]:
try:
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
x1 = int(x0 + 500 * (-b))
y1 = int(y0 + 500 * (a))
def annotate_image(img_in):
"""
Annotate the input image with lane line markings
Returns annotated image
"""
global mtx, dist, left_line, right_line, detected
global left_curve, right_curve, left_lane_inds, right_lane_inds
# Undistort, threshold, perspective transform
undist = cv2.undistort(img_in, mtx, dist, None, mtx)
img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist)
binary_warped, binary_unwarped, m, m_inv = perspective_transform(img)
cv2.imwrite("image.jpg",binary_warped)
# print("saved image")
# print(np.unique(img))
# print(np.unique(binary_warped))
# Perform polynomial fit
if not detected:
# print "Line53"
# Slow line fit
ret = line_fit(binary_warped)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Get moving average of line fit coefficients
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
# calculatingte curvature
# print("HERE")
#left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy)
detected = True # slow line fit always detects the line
else: # implies detected == True
# Fast line fit
left_fit = left_line.get_fit()
right_fit = right_line.get_fit()
ret = tune_fit(binary_warped, left_fit, right_fit)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Only make updates if we detected lines in current frame
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
# print("HERE")
#left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy)
else:
detected = False
vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit)
# print (vehicle_offset)
# Perform final visualization on top of original undistorted image
result = final_viz(undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset)
return result
img = cv2.undistort(img, mtx, dist, None, mtx)
plt.imshow(img)
plt.savefig('example_images/undistort_' + out_image_file)
# Thresholded binary image
img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(img)
plt.imshow(img, cmap='gray', vmin=0, vmax=1)
plt.savefig('example_images/binary_' + out_image_file)
# Perspective transform
img, binary_unwarped, m, m_inv = perspective_transform(img)
plt.imshow(img, cmap='gray', vmin=0, vmax=1)
plt.savefig('example_images/warped_' + out_image_file)
# Polynomial fit
ret = line_fit(img)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
save_file = 'example_images/polyfit_' + out_image_file
viz2(img, ret, save_file=save_file)
# Do full annotation on original image
# Code is the same as in 'line_fit_video.py'
orig = mpimg.imread('test_images/' + image_file)
undist = cv2.undistort(orig, mtx, dist, None, mtx)
left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds,
nonzerox, nonzeroy)
def annotate_image(img_in):
"""
使用车道线标记标注输入图像
返回带注释的图像
"""
detectedfast = False # 根据上一帧结果拟合检测线，省略每次求直方图
T = 0.8
global mtx, dist, left_line, right_line, detected, arrfitL, arrfitR
global left_curve, right_curve, left_lane_inds, right_lane_inds
global top_x_left_history, top_x_right_history, bottom_x_left_history, bottom_x_right_history
global left_LaneFit_history, right_LaneFit_history
# Undistort未失真, threshold阈值, perspective transform视角转换
# undist = cv2.undistort(img_in, mtx, dist, None, mtx)
undist = img_in
# 边缘检测阈值
img, abs_bin, mag_bin, dir_bin, WY_BIN = combined_thresh(undist)
# 透视变换
binary_warped, binary_unwarped, m, m_inv = perspective_transform(img)
# cv2.imshow('img', undist)
# cv2.imshow('binary_warped', binary_warped)
# Perform polynomial fit执行多项式拟合
if not detected:
# Slow line fit
ret = line_fit(binary_warped, T)
if len(ret) > 0:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
out_img = ret['out_img']
histo = ret['histo']
# Get moving average of line fit coefficients
# 获得线性拟合系数的移动平均值
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
# Calculate curvature计算曲率
left_curve, right_curve = calc_curve(left_lane_inds,
right_lane_inds, nonzerox,
nonzeroy)
if detectedfast:
detected = True # slow line fit always detects the line
else:
return img_in
else: # implies detected == True
# Fast line fit
left_fit = left_line.get_fit()
right_fit = right_line.get_fit()
ret = tune_fit(binary_warped, left_fit, right_fit)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Only make updates if we detected lines in current frame
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
left_curve, right_curve = calc_curve(left_lane_inds,
right_lane_inds, nonzerox,
nonzeroy)
else:
detected = False
# 偏移量
vehicle_offset, bottom_x_left, bottom_x_right, top_x_left, top_x_right = calc_vehicle_offset(
undist, left_fit, right_fit)
"""
PMH：采用历史帧更新数据
"""
top_x_left_history, bottom_x_left_history, left_LaneFit_history, is_left_filtered = filter_history(
top_x_left_history, bottom_x_left_history, left_LaneFit_history,
top_x_left, bottom_x_left, left_fit)
top_x_right_history, bottom_x_right_history, right_LaneFit_history, is_right_filtered = filter_history(
top_x_right_history, bottom_x_right_history, right_LaneFit_history,
top_x_right, bottom_x_right, right_fit)
if is_left_filtered is True:
print('左车道线被过滤！')
left_fit = left_LaneFit_history.get_latest()
else:
left_fit = left_LaneFit_history.get_smoothed()
if is_right_filtered is True:
print('右车道线被过滤！')
right_fit = right_LaneFit_history.get_latest()
else:
right_fit = right_LaneFit_history.get_smoothed()
# # 存储左车道线(曲线的参数a,b,c、曲率半径、车道偏移量、图片序号)
# tupcurveL = (left_curve, vehicle_offset, bottom_x_left, num_img)
# tupcurveL = (left_curve, bottom_x_left)
# newleft = left_fit + tupcurveL
# arrfitL.append(newleft)
#
# # 存储右车道线(曲线的参数a,b,c、曲率半径、车道偏移量、图片序号)
# # tupcurveR = (right_curve, vehicle_offset, bottom_x_right, num_img)
# tupcurveR = (right_curve, bottom_x_right)
# newright = right_fit + tupcurveR
# arrfitR.append(newright)
# Perform final visualization on top of original undistorted image
result, color_warp, new_warp, newwarpNO = final_viz(
undist, left_fit, right_fit, m_inv, left_curve, right_curve,
vehicle_offset, is_left_filtered, is_right_filtered)
# cv2.imshow('result', result)
# viz1(binary_warped, ret, save_file=None)
# return img, WY_BIN, binary_unwarped, binary_warped, color_warp, result, arrfitL, arrfitR, new_warp, newwarpNO, out_img, histo
# return arrfitL, arrfitR, new_warp, newwarpNO, color_warp, result, out_img, histo
return result
def annotate_image(img_in):
"""
Annotate the input image with lane line markings
Returns annotated image
"""
global mtx, dist, left_line, right_line, detected, frameCount, retLast
global left_curve, right_curve, left_lane_inds, right_lane_inds
frameCount += 1
src = np.float32(
[[200, 720],
[1100, 720],
[520, 500],
[760, 500]])
x = [src[0, 0], src[1, 0], src[3, 0], src[2, 0], src[0, 0]]
y = [src[0, 1], src[1, 1], src[3, 1], src[2, 1], src[0, 1]]
# Undistort, threshold, perspective transform
undist = cv2.undistort(img_in, mtx, dist, None, mtx)
img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist)
binary_warped, binary_unwarped, m, m_inv = perspective_transform(img)
# Perform polynomial fit
if not detected:
# Slow line fit
ret = line_fit(binary_warped)
# if detect no lanes, use last result instead.
if len(ret) == 0:
ret = retLast
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
out_img = ret['out_img']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
histogram = ret['histo']
# Get moving average of line fit coefficients
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
# Calculate curvature
left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy)
detected = True # slow line fit always detects the line
else: # implies detected == True
# Fast line fit
left_fit = left_line.get_fit()
right_fit = right_line.get_fit()
ret = tune_fit(binary_warped, left_fit, right_fit)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Only make updates if we detected lines in current frame
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy)
else:
detected = False
vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit)
# Perform final visualization on top of original undistorted image
result = final_viz(undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset)
retLast = ret
save_viz2 = './output_images/polyfit_test%d.jpg' % (frameCount)
viz2(binary_warped, ret, save_viz2)
save_warped = './output_images/warped_test%d.jpg' % (frameCount)
plt.imshow(binary_warped, cmap='gray', vmin=0, vmax=1)
if save_warped is None:
plt.show()
else:
plt.savefig(save_warped)
plt.gcf().clear()
save_binary = './output_images/binary_test%d.jpg' % (frameCount)
plt.imshow(img, cmap='gray', vmin=0, vmax=1)
if save_binary is None:
plt.show()
else:
plt.savefig(save_binary)
plt.gcf().clear()
if frameCount > 0:
fig = plt.gcf()
fig.set_size_inches(16.5, 8.5)
plt.subplot(2, 3, 1)
plt.imshow(undist)
# plt.plot(undist)
plt.plot(x, y)
plt.title('undist')
plt.subplot(2, 3, 2)
plt.imshow(hls_bin, cmap='gray', vmin=0, vmax=1)
plt.title('hls_bin')
plt.subplot(2, 3, 3)
plt.imshow(abs_bin, cmap='gray', vmin=0, vmax=1)
plt.title('abs_bin')
plt.subplot(2, 3, 4)
plt.imshow(img, cmap='gray', vmin=0, vmax=1)
plt.title('img')
plt.subplot(2, 3, 5)
plt.imshow(out_img)
plt.title('out_img')
plt.subplot(2, 3, 6)
plt.imshow(result, cmap='gray', vmin=0, vmax=1)
plt.title('result')
save_result = 'D:/code/github_code/CarND-Advanced-Lane-Lines-P4/output_images/result-test%d.jpg' % (frameCount)
if save_result is None:
plt.show()
else:
plt.savefig(save_result)
plt.gcf().clear()
return result
def annotate_frame(self, frame):
#global mtx, dist, left_line, right_line, detected
#global left_curve, right_curve, left_lane_inds, right_lane_inds
# Undistort, threshold, perspective transform
undist = cv2.undistort(frame, self.mtx, self.dist, None, self.mtx)
img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist)
binary_warped, binary_unwarped, m, m_inv = perspective_transform(img)
# Perform polynomial fit
if not self.detected:
# Slow line fit
ret = line_fit(binary_warped)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Get moving average of line fit coefficients
left_fit = self.left_line.add_fit(left_fit)
right_fit = self.right_line.add_fit(right_fit)
# Calculate curvature
self.left_curve, self.right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy)
self.detected = True # slow line fit always detects the line
else: # implies detected == True
# Fast line fit
left_fit = self.left_line.get_fit()
right_fit = self.right_line.get_fit()
ret = tune_fit(binary_warped, left_fit, right_fit)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Only make updates if we detected lines in current frame
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
left_fit = self.left_line.add_fit(left_fit)
right_fit = self.right_line.add_fit(right_fit)
self.left_curve, self.right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy)
else:
self.detected = False
vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit)
# Perform final visualization on top of original undistorted image
result = final_viz(undist, left_fit, right_fit, m_inv, self.left_curve, self.right_curve, vehicle_offset)
return result
def annotate_image(img_in): """ Annotate the input image with lane line markings Returns annotated image """ global mtx, dist, left_line, right_line, detected global left_curve, right_curve, left_lane_inds, right_lane_inds # Undistort, threshold, perspective transform undist = cv2.undistort(img_in, mtx, dist, None, mtx) img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist) binary_warped, binary_unwarped, m, m_inv = perspective_transform(img) # Perform polynomial fit if not detected: # Slow line fit ret = line_fit(binary_warped) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Get moving average of line fit coefficients left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) # Calculate curvature left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) detected = True # slow line fit always detects the line else: # implies detected == True # Fast line fit left_fit = left_line.get_fit() right_fit = right_line.get_fit() ret = tune_fit(binary_warped, left_fit, right_fit) left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] # Only make updates if we detected lines in current frame if ret is not None: left_fit = ret['left_fit'] right_fit = ret['right_fit'] nonzerox = ret['nonzerox'] nonzeroy = ret['nonzeroy'] left_lane_inds = ret['left_lane_inds'] right_lane_inds = ret['right_lane_inds'] left_fit = left_line.add_fit(left_fit) right_fit = right_line.add_fit(right_fit) left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy) else: detected = False vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit) # Perform final visualization on top of original undistorted image result = final_viz(undist, left_fit, right_fit, m_inv, left_curve, right_curve, vehicle_offset) return result
def detection(self, img):
binary_img = self.combinedBinaryImage(img)
img_birdeye, M, Minv = self.perspective_transform(binary_img)
if not self.hist:
# Fit lane without previous result
ret = line_fit(img_birdeye)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
else:
# Fit lane with previous result
if not self.detected:
ret = line_fit(img_birdeye)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
left_fit = self.left_line.add_fit(left_fit)
right_fit = self.right_line.add_fit(right_fit)
self.detected = True
else:
left_fit = self.left_line.get_fit()
right_fit = self.right_line.get_fit()
ret = tune_fit(img_birdeye, left_fit, right_fit)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
left_fit = self.left_line.add_fit(left_fit)
right_fit = self.right_line.add_fit(right_fit)
else:
self.detected = False
# Annotate original image
bird_fit_img = bird_fit(img_birdeye, ret, save_file=None)
combine_fit_img = final_viz(img, left_fit, right_fit, Minv)
return combine_fit_img, bird_fit_img
def get_offset(img_in):
"""
Annotate the input image with lane line markings
Returns annotated image
"""
global mtx, dist, left_line, right_line, detected
global left_curve, right_curve, left_lane_inds, right_lane_inds
# Undistort, threshold, perspective transform
undist = cv2.undistort(img_in, mtx, dist, None, mtx)
img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(undist)
binary_warped, binary_unwarped, m, m_inv = perspective_transform(img)
# Perform polynomial fit
if not detected:
# Slow line fit
ret = line_fit(binary_warped)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Get moving average of line fit coefficients
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
# Calculate curvature
left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds,
nonzerox, nonzeroy)
detected = True # slow line fit always detects the line
else: # implies detected == True
# Fast line fit
left_fit = left_line.get_fit()
right_fit = right_line.get_fit()
ret = tune_fit(binary_warped, left_fit, right_fit)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Only make updates if we detected lines in current frame
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
left_curve, right_curve = calc_curve(left_lane_inds,
right_lane_inds, nonzerox,
nonzeroy)
else:
detected = False
vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit)
float_offset_value = vehicle_offset.item()
#Return the offset in type 'float'.
#vehicle_offset is a numpy.Float64 but we just want the float value to be able to publish
return float_offset_value
image_files=['391_igvcw.png']
for image_file in image_files:
out_image_file = image_file.split('.')[0] + '.png' # write to png format
image_file=(os.path.dirname(os.path.abspath(__file__))+'/saves/' + image_file)
img = mpimg.imread(image_file)
if img.dtype == 'float32':
img = np.array(img)*255
img = np.uint8(img)
img = cv2.blur(img, (5,5))
#img = cv2.undistort(img, mtx, dist, None, mtx)
img2, abs_bin, mag_bin, dir_bin, hls_bin= combined_thresh(img)
#img, _, img2 = combined_thresh_canny(img)
img3, binary_unwarped, m, m_inv = perspective_transform(img2)
ret = line_fit(img3, viz=1)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
save_file = os.path.dirname(os.path.abspath(__file__))+'/saves/polyfit1_' + out_image_file
img4=viz2(img3, ret, save_file=save_file)
# Do full annotation on original image
# Code is the same as in 'line_fit_video.py'
undist = img
left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy)
bottom_y = undist.shape[0] - 1
def annotate_image(img_in, src, dst, detectedfast, T, num_img):
"""
使用车道线标记标注输入图像
返回带注释的图像
"""
global mtx, dist, left_line, right_line, detected, arrfitL, arrfitR
global left_curve, right_curve, left_lane_inds, right_lane_inds
# Undistort未失真, threshold阈值, perspective transform视角转换
# undist = cv2.undistort(img_in, mtx, dist, None, mtx)
undist = img_in
# 边缘检测阈值
img, abs_bin, mag_bin, dir_bin, WY_BIN = combined_thresh(undist)
# 透视变换
binary_warped, binary_unwarped, m, m_inv = perspective_transform(
img, src, dst)
# Perform polynomial fit执行多项式拟合
if not detected:
# Slow line fit
ret = line_fit(binary_warped, T)
if len(ret) > 0:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Get moving average of line fit coefficients
# 获得线性拟合系数的移动平均值
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
# Calculate curvature计算曲率
left_curve, right_curve = calc_curve(left_lane_inds,
right_lane_inds, nonzerox,
nonzeroy)
if detectedfast:
detected = True # slow line fit always detects the line
else:
return img_in
else: # implies detected == True
# Fast line fit
left_fit = left_line.get_fit()
right_fit = right_line.get_fit()
ret = tune_fit(binary_warped, left_fit, right_fit)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
# Only make updates if we detected lines in current frame
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
left_fit = left_line.add_fit(left_fit)
right_fit = right_line.add_fit(right_fit)
left_curve, right_curve = calc_curve(left_lane_inds,
right_lane_inds, nonzerox,
nonzeroy)
else:
detected = False
# 偏移量
vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit)
# 存储左车道线(曲线的参数a,b,c、曲率半径、车道偏移量、图片序号)
tupcurveL = (left_curve, vehicle_offset, num_img)
newleft = left_fit + tupcurveL
arrfitL.append(newleft)
# 存储右车道线(曲线的参数a,b,c、曲率半径、车道偏移量、图片序号)
tupcurveR = (right_curve, vehicle_offset, num_img)
newright = right_fit + tupcurveR
arrfitR.append(newright)
# Perform final visualization on top of original undistorted image
result, color_warp, new_warp, newwarpNO = final_viz(
undist, left_fit, right_fit, m_inv, left_curve, right_curve,
vehicle_offset)
# viz1(binary_warped, ret, save_file=None)
return img, binary_unwarped, binary_warped, color_warp, result, arrfitL, arrfitR, new_warp, newwarpNO
def annotate_image(img_in):
"""
Annotate the input image with lane line markings
Returns annotated image
"""
# Get Frames and Image properties
g.frames = g.frames + 1
g.vi_width = img_in.shape[1]
g.vi_height = img_in.shape[0]
# Undistort, threshold, perspective transform
# Transforms an image to compensate for lens distortion.
# Python: cv2.undistort(src, cameraMatrix, distCoeffs[, dst[, newCameraMatrix]]) -> dst
# src - Input (distorted) image.
# dst - Output (corrected) image that has the same size and type as src .
# cameraMatrix - Input camera matrix A = Matrix(3,3) [[fx,0,cx], [0,fy,cy] [0,0,1]
# distCoeffs - Input vector of distortion coefficients (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]]) of 4, 5, or 8 elements.
# --> If the vector is NULL/empty, the zero distortion coefficients are assumed.
# newCameraMatrix - Camera matrix of the distorted image. By default,
# --> it is the same as cameraMatrix but you may additionally scale and shift the result by using a different matrix.
if (RLD_ZERO_DISTORSION != True):
undist = cv2.undistort(img_in, g.mtx, g.dist, None, g.mtx)
# print ('DISTORSION CORRECTED undist: ', undist)
else:
undist = cv2.undistort(img_in, g.mtx, None, None, g.mtx)
# print ('DISTORSION NOT CORRECTED undist: ', undist)
# Display undistort image
if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2:
if g.cold_boot:
cv2.namedWindow('undist_image', cv2.WINDOW_NORMAL)
cv2.resizeWindow('undist_image', g.s_win_width, g.s_win_height)
cv2.moveWindow('undist_image', 0, g.s_height // 2 + g.s_win_height)
undist_bgr = cv2.cvtColor(undist, cv2.COLOR_RGB2BGR)
cv2.imshow('undist_image', undist_bgr)
# Combine all threshold mask on the undistort image
#histogram_calc(undist)
img, abs_bin, mag_bin, dir_bin, hls_bin, hsv_bin = combined_thresh(undist)
#img = combined_canny(undist)
if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2:
if g.cold_boot:
cv2.namedWindow('undist_comb_thresh', cv2.WINDOW_NORMAL)
cv2.resizeWindow('undist_comb_thresh', g.s_win_width,
g.s_win_height)
cv2.moveWindow(
'undist_comb_thresh', 3 * g.s_win_width, 2 * g.s_win_height -
g.s_win_height // 2 + 2 * g.s_win_height_offset)
cv2.imshow('undist_comb_thresh', img)
# Apply perpective transformation/warp binary image
binary_warped, binary_unwarped, m, m_inv, src, dst = perspective_transform(
img)
unwarped_trapez = (np.dstack(
(binary_unwarped, binary_unwarped, binary_unwarped)) *
255).astype('uint8')
if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2:
if g.cold_boot:
cv2.namedWindow('warped_image', cv2.WINDOW_NORMAL)
cv2.resizeWindow('warped_image', g.s_win_width, g.s_win_height)
cv2.moveWindow('warped_image', 4 * g.s_win_width,
3 * g.s_win_height)
cv2.imshow('warped_image', binary_warped)
if g.cold_boot:
cv2.namedWindow('unwarped_image', cv2.WINDOW_NORMAL)
cv2.resizeWindow('unwarped_image', g.s_win_width, g.s_win_height)
cv2.moveWindow('unwarped_image', 3 * g.s_win_width,
3 * g.s_win_height)
if g.trackbar_enabled:
cv2.createTrackbar('top', 'unwarped_image',
int(g.trap_top_width * 100), 100, nothing)
cv2.createTrackbar('bottom', 'unwarped_image',
int(g.trap_bottom_width * 100), 100,
nothing)
cv2.createTrackbar('height', 'unwarped_image',
int(g.trap_height * 100), 100, nothing)
cv2.setTrackbarPos('top', 'unwarped_image',
int(g.trap_top_width * 100))
cv2.setTrackbarPos('bottom', 'unwarped_image',
int(g.trap_bottom_width * 100))
cv2.setTrackbarPos('height', 'unwarped_image',
int(g.trap_height * 100))
if g.trackbar_enabled:
l_top = cv2.getTrackbarPos('top', 'unwarped_image')
g.trap_top_width = float(l_top) / 100
l_bottom = cv2.getTrackbarPos('bottom', 'unwarped_image')
g.trap_bottom_width = float(l_bottom) / 100
l_height = cv2.getTrackbarPos('height', 'unwarped_image')
g.trap_height = float(l_height) / 100
cv2.polylines(unwarped_trapez, np.int32([src]), True, (255, 255, 0), 1,
0)
cv2.imshow('unwarped_image', unwarped_trapez)
# Perform polynomial fit
left_fit = None
right_fit = None
if not g.detected:
# Slow line fit
ret = line_fit(binary_warped)
#print ('ret:', ret)
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
g.left_lane_inds = ret['left_lane_inds']
g.right_lane_inds = ret['right_lane_inds']
# Get moving average of line fit coefficients
left_fit = g.left_line.add_fit(left_fit)
right_fit = g.right_line.add_fit(right_fit)
# Calculate curvature
g.left_curve, g.right_curve = calc_curve(g.left_lane_inds,
g.right_lane_inds,
nonzerox, nonzeroy)
if g.detect_fast_mode_allowed:
g.detected = True # slow line fit always detects the line
else:
g.detected = False # Force the slow mode for ever
if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2:
viz1(binary_warped, ret, save_file=None)
viz2(binary_warped, ret, save_file=None)
else:
if not g.degraded_viz_mode:
g.detected = False
else: # implies g.detected == True
# Fast line fit
left_fit = g.left_line.get_fit()
right_fit = g.right_line.get_fit()
ret = tune_fit(binary_warped, left_fit, right_fit)
# Only make updates if we detected lines in current frame
if ret is not None:
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
g.left_lane_inds = ret['left_lane_inds']
g.right_lane_inds = ret['right_lane_inds']
left_fit = g.left_line.add_fit(left_fit)
right_fit = g.right_line.add_fit(right_fit)
g.left_curve, g.right_curve = calc_curve(g.left_lane_inds,
g.right_lane_inds,
nonzerox, nonzeroy)
if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2:
#viz1(binary_warped, ret, save_file=None)
viz2(binary_warped, ret, save_file=None)
else:
if not g.degraded_viz_mode:
g.detected = False
if ret is not None:
vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit)
# Perform final visualization on top of original undistorted image
# print ('g.detected:',g.detected ,'g.left_curve:',g.left_curve ,'g.right_curve:',g.right_curve)
# print ('left_fit:',left_fit ,'right_fit:',right_fit)
result = final_viz(undist, left_fit, right_fit, m_inv, g.left_curve,
g.right_curve, vehicle_offset)
else:
if g.degraded_viz_mode and g.left_line.len_fit(
) != 0 and g.right_line.len_fit() != 0:
if left_fit is None or right_fit is None:
left_fit = g.left_line.get_fit()
right_fit = g.right_line.get_fit()
vehicle_offset = calc_vehicle_offset(undist, left_fit, right_fit)
result = final_viz(undist, left_fit, right_fit, m_inv,
g.left_curve, g.right_curve, vehicle_offset)
else:
result = undist
if DEBUG_LINE_FIT_VIDEO >= DEBUG_LEVEL2:
if g.cold_boot:
cv2.namedWindow('final_visu', cv2.WINDOW_NORMAL)
cv2.resizeWindow('final_visu', 640, 480)
cv2.putText(result, 'Filter: ' + g.combined_filter_type,
(1, int(3 * g.vi_height * VI_TEXT_OFFSET / VI_HEIGHT)),
cv2.FONT_HERSHEY_SIMPLEX,
g.vi_height * VI_FONT_SIZE / VI_HEIGHT, (0, 255, 0), 1,
cv2.LINE_AA)
cv2.putText(
result, 'Res: ' + str(g.vi_width) + 'x' + str(g.vi_height) +
' - Frame:' + str(g.frames),
(1, int(4 * g.vi_height * VI_TEXT_OFFSET / VI_HEIGHT)),
cv2.FONT_HERSHEY_SIMPLEX, g.vi_height * VI_FONT_SIZE / VI_HEIGHT,
(255, 0, 0), 1, cv2.LINE_AA)
cv2.putText(
result,
'pX: ' + str(round(g.scale_px_width, 2)) + ' mm/px' + ' l_size:' +
str(round(g.lane_size_px * g.scale_px_width / 1000, 2)) + ' m',
(1, int(5 * g.vi_height * VI_TEXT_OFFSET / VI_HEIGHT)),
cv2.FONT_HERSHEY_SIMPLEX, g.vi_height * VI_FONT_SIZE / VI_HEIGHT,
(255, 0, 0), 1, cv2.LINE_AA)
cv2.putText(result,
g.detect_mode + ' Recovery: ' + str(g.degraded_viz_count),
(1, int(6 * g.vi_height * VI_TEXT_OFFSET / VI_HEIGHT)),
cv2.FONT_HERSHEY_SIMPLEX,
g.vi_height * VI_FONT_SIZE / VI_HEIGHT, (0, 255, 255), 1,
cv2.LINE_AA)
result_bgr = cv2.cvtColor(result, cv2.COLOR_RGB2BGR)
cv2.imshow('final_visu', result_bgr)
if cv2.waitKey(1) & 0xFF == ord('q'):
sys.exit(0)
g.cold_boot = False
return result
img = cv2.undistort(img, mtx, dist, None, mtx)
plt.imshow(img)
plt.savefig('example_images/undistort_' + out_image_file)
# Thresholded binary image
img, abs_bin, mag_bin, dir_bin, hls_bin = combined_thresh(img)
plt.imshow(img, cmap='gray', vmin=0, vmax=1)
plt.savefig('example_images/binary_' + out_image_file)
# Perspective transform
img, binary_unwarped, m, m_inv = perspective_transform(img)
plt.imshow(img, cmap='gray', vmin=0, vmax=1)
plt.savefig('example_images/warped_' + out_image_file)
# Polynomial fit
ret = line_fit(img)
left_fit = ret['left_fit']
right_fit = ret['right_fit']
nonzerox = ret['nonzerox']
nonzeroy = ret['nonzeroy']
left_lane_inds = ret['left_lane_inds']
right_lane_inds = ret['right_lane_inds']
save_file = 'example_images/polyfit_' + out_image_file
viz2(img, ret, save_file=save_file)
# Do full annotation on original image
# Code is the same as in 'line_fit_video.py'
orig = mpimg.imread('test_images/' + image_file)
undist = cv2.undistort(orig, mtx, dist, None, mtx)
left_curve, right_curve = calc_curve(left_lane_inds, right_lane_inds, nonzerox, nonzeroy)
