print('Left lane coefficients : ')
print(lc)
lfit = np.poly1d(lc)
if not len(rightx) is 0:
right_fit = np.polyfit(righty, rightx, 2)
rc, res, _, _, _ = np.polyfit(righty, rightx, 2, full=True)
print('Right lane coefficients : ')
print(rc)
rfit = np.poly1d(rc)
out_img = cv2.resize(out_img, (640, 360))
cv2.imshow('out_img', out_img)
if not len(leftx) is 0:
left_fit = left_line.add_fit(left_fit)
if not len(rightx) is 0:
right_fit = right_line.add_fit(right_fit)
if not len(leftx) is 0:
y_eval = 719
ym_per_pix = 5 / 720
xm_per_pix = 0.3 / 1250
left_curverad = (
(1 + (2 * left_fit[0] * y_eval * ym_per_pix + left_fit[1])**2)**
1.5) / np.absolute(2 * left_fit[0])
right_curverad = (
(1 + (2 * right_fit[0] * y_eval * ym_per_pix + right_fit[1])**2)**
1.5) / np.absolute(2 * right_fit[0])
if not len(leftx) is 0:
class FindLaneLine():
def __init__(self, window_size, input_video, output_video):
# init Camera
with open('calibrate_camera.p', 'rb') as f:
save_dict = pickle.load(f)
print("Load calibrate_camera pickle -- done")
self.input_video = input_video
self.mtx = save_dict['mtx']
self.dist = save_dict['dist']
self.window_size = window_size
self.left_line = Line(n=window_size)
self.right_line = Line(n=window_size)
# init detect lane False
self.detected = False
self.left_curve, self.right_curve = 0., 0. # radius of curvature for left and right lanes
self.left_lane_curve, self.right_lane_curve = None, None # for calculating curvature
# Annotated Input Video
annotated_v = self.load_video(input_video)
# generate output Video
annotated_v.write_videofile(output_video, audio=False)
def load_video(self, input_video):
""" Given input_file video, save annotated video to output_file """
video = VideoFileClip(input_video)
return video.fl_image(self.annotate_frame)
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
# wrap imgs
wrapped = pipeline.wrap(gradient)
# find left and right pixel poses
# find left and right polyline
left_line.count_check()
right_line.count_check()
lx, ly, rx, ry = pipeline.find_lines(wrapped)
if len(rx) > 0 and len(ry) > 0:
# set was detected
right_line.was_detected = True
# add founded points to instanse
right_line.allx = rx
right_line.ally = ry
# add poly
l_fit, right_line.polx, right_line.ploty = pipeline.find_poly(wrapped.shape,right_line.allx, right_line.ally)
right_line.add_fit(l_fit)
# if find points of left line
if len(lx) > 0 and len(ly) > 0:
# set was detected
left_line.was_detected = True
# add founded points to instanse
left_line.allx = lx
left_line.ally = ly
# add poly
r_fit, left_line.polx, left_line.ploty = pipeline.find_poly(wrapped.shape,left_line.allx, left_line.ally)
left_line.add_fit(r_fit)
left_curvd = pipeline.measure_curv(wrapped.shape, lx, ly)
right_curvd = pipeline.measure_curv(wrapped.shape, rx, ry)
avg_curvrd = round(np.mean([left_curvd,right_curvd]), 0)
curv_text = f"current radius of curvature is {avg_curvrd} meters"
class lanenet_detector():
def __init__(self):
self.bridge = CvBridge()
self.sub_image = rospy.Subscriber('camera/image_raw',
Image,
self.img_callback,
queue_size=1)
self.pub_image = rospy.Publisher("lane_detection/annotate",
Image,
queue_size=1)
self.pub_bird = rospy.Publisher("lane_detection/birdseye",
Image,
queue_size=1)
self.left_line = Line(n=5)
self.right_line = Line(n=5)
self.detected = False
self.hist = True
def img_callback(self, data):
try:
# Convert a ROS image message into an OpenCV image
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
raw_img = cv_image.copy()
mask_image, bird_image = self.detection(raw_img)
# Convert an OpenCV image into a ROS image message
out_img_msg = self.bridge.cv2_to_imgmsg(mask_image, 'bgr8')
out_bird_msg = self.bridge.cv2_to_imgmsg(bird_image, 'bgr8')
# Publish image message in ROS
self.pub_image.publish(out_img_msg)
self.pub_bird.publish(out_bird_msg)
def gradient_thresh(self, img, thresh_min=25, thresh_max=100):
"""
Apply sobel edge detection on input image in x, y direction
"""
#1. Convert the image to gray scale
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
#2. Gaussian blur the image
gray = cv2.GaussianBlur(gray, (5, 5), 0)
#3. Use cv2.Sobel() to find derievatives for both X and Y Axis
sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0) #, ksize=1)
sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1) #, ksize=1)
#4. Use cv2.addWeighted() to combine the results
gray = cv2.addWeighted(sobelx, 0.5, sobely, 0.5, 0)
#5. Convert each pixel to unint8, then apply threshold to get binary image
gray = cv2.convertScaleAbs(gray)
num, binary_output = cv2.threshold(gray, thresh_min, thresh_max,
cv2.THRESH_BINARY)
# binary_output = gray
return binary_output
def color_thresh(self, img, thresh=(100, 255)):
"""
Convert RGB to HSL and threshold to binary image using S channel
"""
#1. Convert the image from RGB to HSL
hsl = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
#2. Apply threshold on S channel to get binary image
binary_output = cv2.inRange(hsl, (0, 0, thresh[0]),
(255, 255, thresh[1]))
return binary_output
def combinedBinaryImage(self, img):
"""
Get combined binary image from color filter and sobel filter
"""
#1. Apply sobel filter and color filter on input image
SobelOutput = self.gradient_thresh(img)
ColorOutput = self.color_thresh(img)
#2. Combine the outputs
# SobelOutput = cv2.addWeighted(SobelOutput, 0.5, ColorOutput, 0.5, 0)
## Here you can use as many methods as you want.
## TODO
####
binaryImage = np.zeros_like(SobelOutput)
binaryImage[(ColorOutput != 0) | (SobelOutput != 0)] = 1
# Remove noise from binary image
binaryImage = morphology.remove_small_objects(
binaryImage.astype('bool'), min_size=50, connectivity=2)
binaryImage = binaryImage.astype(np.uint8) * 255
return binaryImage
def perspective_transform(self, img, verbose=False):
"""
Get bird's eye view from input image
"""
#1. Visually determine 4 source points and 4 destination points
src_pts = np.array([[480, 250], [680, 250], [809, 370], [292, 370]],
np.float32)
dst_pts = np.array([[292, 0], [809, 0], [809, 370], [292, 370]],
np.float32)
#2. Get M, the transform matrix, and Minv, the inverse using cv2.getPerspectiveTransform()
M = cv2.getPerspectiveTransform(src_pts, dst_pts)
Minv = np.linalg.inv(M)
#3. Generate warped image in bird view using cv2.warpPerspective()
warped_img = cv2.warpPerspective(img, M, (1242, 375))
## TODO
####
return warped_img, M, Minv
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
class lanenet_detector():
def __init__(self):
self.bridge = CvBridge()
self.sub_image = rospy.Subscriber('camera/image_raw', Image, self.img_callback, queue_size=1)
self.pub_image = rospy.Publisher("lane_detection/annotate", Image, queue_size=1)
self.pub_bird = rospy.Publisher("lane_detection/birdseye", Image, queue_size=1)
self.left_line = Line(n=5)
self.right_line = Line(n=5)
self.detected = False
self.hist = True
def img_callback(self, data):
try:
# Convert a ROS image message into an OpenCV image
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
raw_img = cv_image.copy()
mask_image, bird_image = self.detection(raw_img)
# Convert an OpenCV image into a ROS image message
out_img_msg = self.bridge.cv2_to_imgmsg(mask_image, 'bgr8')
out_bird_msg = self.bridge.cv2_to_imgmsg(bird_image, 'bgr8')
# Publish image message in ROS
self.pub_image.publish(out_img_msg)
self.pub_bird.publish(out_bird_msg)
def gradient_thresh(self, img, thresh_min=25, thresh_max=100):
"""
Apply sobel edge detection on input image in x, y direction
"""
# 1. Convert the image to gray scale
gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# 2. Gaussian blur the image
blur_img = cv2.GaussianBlur(gray_img, (5, 5), 0)
# 3. Use cv2.Sobel() to find derievatives for both X and Y Axis
sobelX = cv2.Sobel(blur_img, cv2.CV_64F, 1, 0, ksize=1)
sobelY = cv2.Sobel(blur_img, cv2.CV_64F, 0, 1, ksize=1)
#4. Use cv2.addWeighted() to combine the results
blended_img = cv2.addWeighted(np.absolute(sobelX), 0.5, np.absolute(sobelY), 0.5, 0)
#5. Convert each pixel to unint8, then apply threshold to get binary image
uint8_img = np.uint8(blended_img)
ret,binary_output = cv2.threshold(uint8_img,thresh_min,thresh_max,cv2.THRESH_BINARY)
return binary_output
def color_thresh(self, img, thresh=(100, 255)):
"""
Convert RGB to HSL and threshold to binary image using S channel
"""
#1. Convert the image from RGB to HSL
hsl_img = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
#2. Apply threshold on S channel to get binary image
binary_output = cv2.inRange(hsl_img,(0,0,thresh[0]), (255,255,255))
return binary_output
def combinedBinaryImage(self, img):
"""
Get combined binary image from color filter and sobel filter
"""
#1. Apply sobel filter and color filter on input image
SobelOutput = self.gradient_thresh(img)
ColorOutput = self.color_thresh(img)
#2. Combine the outputs
binaryImage = cv2.addWeighted(np.absolute(SobelOutput), 0.5, np.absolute(ColorOutput), 0.5, 0)
# Here you can use as many methods as you want.
binaryImage[(ColorOutput==1)|(SobelOutput==1)] = 1
# Remove noise from binary image
binaryImage = morphology.remove_small_objects(binaryImage.astype(np.uint8()),min_size=50,connectivity=2)
# plt.imshow(img, interpolation='none') # Plot the image, turn off interpolation
# plt.show()
# cv2.imshow("Sobel", SobelOutput)
# cv2.waitKey(0)
# cv2.imshow("Color", ColorOutput)
# cv2.waitKey(0)
# print(SobelOutput, ColorOutput, binaryImage)
# cv2.imshow("Combined", binaryImage)
# cv2.waitKey(0)
return binaryImage
def perspective_transform(self, img, verbose=False):
"""
Get bird's eye view from input image
"""
#1. Visually determine 4 source points and 4 destination points
#2. Get M, the transform matrix, and Minv, the inverse using cv2.getPerspectiveTransform()
#3. Generate warped image in bird view using cv2.warpPerspective()
h,w = img.shape[:2] #width and height
# print(h,w)
OFFSET = 50
# 0011
# x1, y1 = 550, 200 # upper left
# x2, y2 = 200, 375 # lower left
# x3, y3 = 650, 200 # upper right
# x4, y4 = 1000, 375 # lower right
# 0056yellow
# x1, y1 = 500, 225 # upper left
# x2, y2 = 200, 375 # lower left
# x3, y3 = 650, 225 # upper right
# x4, y4 = 800, 375 # lower right
x1, y1 = 555, 195 # upper left
x2, y2 = 630, 195 # lower left
x3, y3 = 950, 375 # upper right
x4, y4 = 200, 375 # lower right
# dst = np.float32([(OFFSET,0),#upper left
# (OFFSET,375),#lower left
# (1242-OFFSET,0),#upper right
# (1242-OFFSET,375)]) #lower right
dst = np.float32([(0,0),#upper left
(1000,0),#lower left
(1000,300),#upper right
(50,300)]) #lower right
src = np.float32([(x1,y1),(x2,y2),(x3,y3),(x4,y4)])
#2. Get M, the transform matrix, and Minv, the inverse using cv2.getPerspectiveTransform()
M = cv2.getPerspectiveTransform(src, dst)
Minv = cv2.getPerspectiveTransform(dst, src)
#3. Generate warped image in bird view using cv2.warpPerspective()
warped_img = cv2.warpPerspective(img, M, (w,h), flags=cv2.INTER_LINEAR)
## TODO
# cv2.imshow("bird", warped_img)
# cv2.waitKey(0)
####
return warped_img, M, Minv
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
