选取大神陈光的高级车道线检测方法进行改造，总体逻辑是：
第一、创建订阅者，接收摄像头发布的数据，用cv_bridge将opencv格式的数据转换为ROS的消息格式数据。
第二、创建发布者，将检测到的数据以特定的消息类型发布出去
传统方法的难点：
（1）对光照、明暗、车道线磨损、非常敏感
（2）在十字路口转弯时，摄像头检测不到前方车道线，会造成绿色区域“变幻莫测”地跳动
（3）总会出现x expect non zero vector错误，导致程序退出
计划对程序的改进
（1）在十字路口等工况下检测不到车道线时，直接return回spin()函数，等待下一帧数据
（2）在进入np.polyfit()函数进行二次曲线拟合时，先判断参数是否为空
（3）在画面上打印FPS和 本车距离坐车道线的距离
#!/usr/bin/env python
# coding=utf-8
import os
import cv2
import matplotlib.pyplot as plt
import numpy as np
from moviepy.editor import VideoFileClip
import glob
import time
import math
import os
os.environ["CUDA_VISIBLE_DEVICES"] = "0"
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
#################################################################
# Step 3 : Warp image based on src_points and dst_points（透视变换）
#################################################################
# The type of src_points & dst_points should be like
# np.float32([ [0,0], [100,200], [200, 300], [300,400]])
def warpImage(image, src_points, dst_points):
image_size = (image.shape[1], image.shape[0])
M = cv2.getPerspectiveTransform(src_points, dst_points)#src_points到dst_points的投影矩阵
Minv = cv2.getPerspectiveTransform(dst_points, src_points)#逆投影矩阵
#flags=cv2.INTER_LINEAR对远离摄像头的部分进行线性插值填充像素点
warped_image = cv2.warpPerspective(image, M,image_size, flags=cv2.INTER_LINEAR)
return warped_image, M, Minv
#################################################################
# Step 4 : 提取车道线
#################################################################
def hlsLSelect(img, thresh=(220, 255)): #HLS通道图，对L（亮度）处理，提取白色车道线
hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS) #print(hls.shape),(1080,1920,3)
l_channel = hls[:,:,1] #print(l_channel.shape),(1080,1920)
#np.max(l_channel),求矩阵中最大的元素，125.#数据的缩放
l_channel = 255*(l_channel/np.max(l_channel)) #np.max(l_channel)=255.0
binary_output = np.zeros_like(l_channel) #创建一个空矩阵，黑图片
binary_output[(l_channel > thresh[0]) & (l_channel <= thresh[1])] = 1 #在阈值范围内的点亮
###cv2.imshow("whitelane",binary_output)
###cv2.waitKey(0)
return binary_output
def labBSelect(img, thresh=(195, 255)):#转为Lab通道的图，随后对b通道进行分割处理，提取图像中黄色的车道线
# 1) Convert to LAB color space
lab = cv2.cvtColor(img, cv2.COLOR_BGR2Lab)
lab_b = lab[:,:,2]
# don't normalize if there are no yellows in the image
if np.max(lab_b) > 100:#162
lab_b = 255*(lab_b/np.max(lab_b))#print(np.max(lab_b)),255.0
# 2) Apply a threshold to the L channel
#print(lab_b)
binary_output = np.zeros_like(lab_b)
binary_output[((lab_b > thresh[0]) & (lab_b <= thresh[1]))] = 1
###cv2.imshow("yellolane",binary_output)
###cv2.waitKey(0)
# 3) Return a binary image of threshold result
return binary_output
#################################################################
# Step 5 : Detect lane lines through moving window(检测车道线)
#################################################################
def find_lane_pixels(binary_warped, nwindows, margin, minpix):
# Create an output image to draw on and visualize the result,可视化
out_img = np.dstack((binary_warped, binary_warped, binary_warped))
# Take a histogram of the bottom half of the image,计算图像下半部分每列上白色像素点之和,shape[0]=1080，//是整除，图像的左上角是坐标原点
histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0)
# 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)##midpoint = 960
leftx_base = np.argmax(histogram[:midpoint])#左车道线起始点,0到960范围内，像素之和最大的横坐标
rightx_base = np.argmax(histogram[midpoint:]) + midpoint#右车道线起始点
# Set height of windows - based on nwindows above and image shape
window_height = np.int(binary_warped.shape[0]//nwindows)
# Identify the x and y positions of all nonzero pixels in the image返回非0元素的索引值
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Current positions to be updated later for each window in nwindows
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 = []
# 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##margin是滑动窗口宽度的一半
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
# Draw the windows on the visualization image
cv2.rectangle(out_img,(win_xleft_low,win_y_low),
(win_xleft_high,win_y_high),(0,255,0), 2)
cv2.rectangle(out_img,(win_xright_low,win_y_low),
(win_xright_high,win_y_high),(0,255,0), 2)
#cv2.imshow("out_img", out_img)
#cv2.waitKey(0)
# 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]
#print(good_left_inds)
#print(good_right_inds)
# 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:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices (previously was a list of lists of pixels)
try:
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
except ValueError:
# Avoids an error if the above is not implemented fully
pass
# 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]
return leftx, lefty, rightx, righty, out_img
def fit_polynomial(binary_warped, nwindows=9, margin=100, minpix=50):
# Find our lane pixels first
leftx, lefty, rightx, righty, out_img = find_lane_pixels(
binary_warped, nwindows, margin, minpix)
print('leftx:', leftx)
print('lefty:', lefty)
print('rightx:', rightx)
print('righty:', righty)
# Fit a second order polynomial to each using `np.polyfit`
if len(leftx) == 0 or len(rightx) == 0:
out_img=0
left_fit=np.array([0, 0, 0])
right_fit=np.array([0, 0, 0])
ploty=0
return out_img, left_fit, right_fit, ploty
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
# Generate x and y values for plotting
ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
#try:
## 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]
##except TypeError:
## # Avoids an error if `left` and `right_fit` are still none or incorrect
## print('The function failed to fit a line!')
## left_fitx = 1*ploty**2 + 1*ploty
## right_fitx = 1*ploty**2 + 1*ploty
## Visualization ##
# Colors in the left and right lane regions
out_img[lefty, leftx] = [255, 0, 0]
out_img[righty, rightx] = [0, 0, 255]
# Plots the left and right polynomials on the lane lines
#plt.plot(left_fitx, ploty, color='yellow')
#plt.plot(right_fitx, ploty, color='yellow')
print('left shape:', left_fit.shape)
print('left type:', type(left_fit))
return out_img, left_fit, right_fit, ploty
#################################################################
# Step 6 : Track lane lines based the latest lane line result(跟踪车道线)
#################################################################
def fit_poly(img_shape, leftx, lefty, rightx, righty):
### TO-DO: Fit a second order polynomial to each with np.polyfit() ###
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
# Generate x and y values for plotting
ploty = np.linspace(0, img_shape[0]-1, img_shape[0])
### TO-DO: Calc both polynomials using ploty, left_fit and right_fit ###
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]
return left_fitx, right_fitx, ploty, left_fit, right_fit
def search_around_poly(binary_warped, left_fit, right_fit):
# HYPERPARAMETER
# Choose the width of the margin around the previous polynomial to search
# The quiz grader expects 100 here, but feel free to tune on your own!
margin = 60
# Grab activated pixels
nonzero = binary_warped.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
### TO-DO: Set the area of search based on activated x-values ###
### within the +/- margin of our polynomial function ###
### Hint: consider the window areas for the similarly named variables ###
### in the previous quiz, but change the windows to our new search area ###
left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy +
left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) +
left_fit[1]*nonzeroy + left_fit[2] + margin)))
right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy +
right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) +
right_fit[1]*nonzeroy + right_fit[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]
# Fit new polynomials
left_fitx, right_fitx, ploty, left_fit, right_fit = fit_poly(binary_warped.shape, leftx, lefty, rightx, righty)
## Visualization ##
# 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))
result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
# Plot the polynomial lines onto the image
#plt.plot(left_fitx, ploty, color='yellow')
#plt.plot(right_fitx, ploty, color='yellow')
## End visualization steps ##
return result, left_fit, right_fit, ploty
#################################################################
# Step 7 : CalculateDistFromCenter
#################################################################
def CalculateDistFromCenter(binary_image, left_fit, right_fit):
img_size = (binary_image.shape[1], binary_image.shape[0])
dist_from_center = 0.0
# assume the camera is centered in the vehicle
###camera_pos = img_size[1] / 2
if right_fit is not None:
if left_fit is not None:
# 摄像头位于图像中间，也是本车的中心
camera_pos = img_size[0] / 2
###RESUBMIT - END
# find where the right and left lanes intersect the bottom of the frame
# 左右车道线最底端x坐标
left_lane_pix = np.polyval(left_fit, binary_image.shape[0])
right_lane_pix = np.polyval(right_fit, binary_image.shape[0])
# 左右车道线中点x坐标
center_of_lane_pix = (left_lane_pix + right_lane_pix) / 2
# 摄像头（本车中心）与车道线中心的距离
dist_from_center = (camera_pos - center_of_lane_pix) * 3.7/1280
#print(dist_from_center, 'm')
return dist_from_center
#################################################################
# Step 8 : Draw lane line result on undistorted image(逆透视变换)
#################################################################
def drawing(undist, bin_warped, color_warp, left_fitx, right_fitx, ploty, Minv):
# Create an image to draw the lines on
warp_zero = np.zeros_like(bin_warped).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
# Warp the blank back to original image space using inverse perspective matrix (Minv)
newwarp = cv2.warpPerspective(color_warp, Minv, (undist.shape[1], undist.shape[0]))
# Combine the result with the original image
result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)
return result
#################################################################
# Step 9 : show text
#################################################################
def overlay_text_on_image (image, dist_from_center, fps):
new_img = np.copy(image)
font = cv2.FONT_HERSHEY_SIMPLEX
font_color = (255,255,255)
num_format = '{:04.2f}'
text = 'FPS: ' + str(fps)
cv2.putText(new_img, text, (40,70), font, 1.5, font_color, 2, cv2.LINE_AA)
direction = 'left'
if dist_from_center > 0:
direction = 'right'
abs_dist = abs(dist_from_center)
text = 'Vehicle is ' + num_format.format(abs_dist) + ' m ' + direction + ' of center'
cv2.putText(new_img, text, (40,120), font, 1.5, font_color, 2, cv2.LINE_AA)
return new_img
###-------------------------------------------------------------------------------------------###
# 左图梯形区域的四个端点,从左上端点开始顺时针方向
src = np.float32([[603, 342], [727, 342], [1150, 720], [225, 720]])
wrap_offset = 150
# 右图矩形区域的四个端点
dst = np.float32([[225+wrap_offset, 0], [1150-wrap_offset, 0], [1150-wrap_offset, 720], [225+wrap_offset, 720]])
bridge = CvBridge()
detected = False
##left_fit = []
##right_fit = []
##ploty = []
def callbackFunc(image):
rospy.loginfo('receive frame success')
global bridge, src, dst, detected
try:
undistort_image = bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print(e)
start = time.time()
#步骤3-透视变换
warp_image, M, Minv = warpImage(undistort_image, src, dst)
###cv2.imshow("warp_image",warp_image)
###cv2.waitKey(0)
#步骤4-提取车道线
hlsL_binary = hlsLSelect(warp_image)
labB_binary = labBSelect(warp_image, (205, 255))
combined_binary = np.zeros_like(hlsL_binary)
combined_binary[(hlsL_binary == 1) | (labB_binary == 1)] = 1
###cv2.imshow("combined_binary",combined_binary)
###cv2.waitKey(0)
left_fit = []
right_fit = []
ploty = []
if detected == False:#步骤5-滑动窗口检测车道线
out_img, left_fit, right_fit, ploty = fit_polynomial(combined_binary, nwindows=9, margin=80, minpix=40)
###cv2.imshow("out_img",out_img)
###cv2.waitKey(0)
###if (len(left_fit) > 0 & len(right_fit) > 0) :
if left_fit[0] == 0 and left_fig[1] == 0:
detected = False
return
else :
detected = True
else:#步骤6-追踪车道线
track_result, left_fit, right_fit, ploty, = search_around_poly(combined_binary, left_fit, right_fit)
if (len(left_fit) > 0 & len(right_fit) > 0) :
detected = True
else :
detected = False
return
end = time.time()
fps = math.floor(1 / (end - start))
# step 7-CalculateDistFromCenter
dist_from_center = CalculateDistFromCenter(warp_image, left_fit, right_fit)
#步骤8-逆透视变换 and drawing
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]
result = drawing(undistort_image, combined_binary, warp_image, left_fitx, right_fitx, ploty, Minv)
#step 9-show text
result = overlay_text_on_image (result, dist_from_center, fps)
cv2.imshow("result",result)
cv2.waitKey(40)
def msg_subscriber():
rospy.init_node('msg_subscriber', anonymous=True)
rospy.Subscriber('/miivii_gmsl_ros/camera3', Image, callbackFunc)
rospy.spin()
if __name__ == '__main__':
msg_subscriber()
（1)追踪车道线的函数中，计算left_lane_inds时出现数组下标溢出错误
（2）提取车道线的颜色空间阈值还没有调整
launch文件里的权重路径，只包含.index .data .meta等后缀前面的前缀。warning:...已过期，用...代替,照着一个一个修改，最后也没全部改完，不改了。。。
qt5参考教程1和教程2eigen参考教程，其中下载安装包的步骤参考教程痛苦如此持久，像蜗牛充满耐心地移动；快乐如此短暂，像兔子的尾巴掠过秋天的草原。
国庆节前一天刷了JetPack4.4，终于运行通了车道线检测的tensorRT加速项目，惊喜之余心凉凉，发现darknet_ros因为OpenCV版本的问题编译不通，于是对OpenCV千般改造miivii域控制器（Xavier）配置ROS与OpenCV3.x.x，运行成功darknet_ros后，发现域控制器自带的GMSL摄像头驱动又编译不通，昨天休整了半天，对目前的困境进行了小结：
JetPack4.4下有4项任务：（1）启动GMSL摄像头（2）demo检测行人、车辆和自行车（3）检测交通标志（4）检测车道线
（1）和（2）的程序依赖刷机时提供的动态库，因此必须用初始的默认版本OpenCV4.1.1。昨天验证了（4），在OpenCV3.2.0和4.1.1下都能运行。我查了一些资料后，发现（3）darknet_ros只在OpenCV3.x.x下运行正常，4.x.x下问题很多，昨晚上github，发现有人尝试在4.x.x下编译darknet_ros,Opencv4, now working #202，结论是将darknet_ros/下的darknet替换为AlexeyAB/darknet，后者在OpenCV2/3/4下都能编译。于是将OpenCV改回4.1.1，尝试编译AlexeyAB/darknet。
AlexeyAB/darknet编译运行成功，替换darknet_ros下的darknet后，catkin_make失败。
用kunaltyagi/darknet替换darknet_ros下的darknet，参照OpenCV4 compilation success修改相应文件，catkin_make仍失败。