def __init__(self):
self.undistorter = Undistorter()
#print("Distortion Initiation Success")
self.thresholder = Thresholder()
#print("Thresholding Initiation Success")
self.ptransformer = PerspectiveTransformer()
#print("Perspective Transformer Initiation Success")
self.laneDetector = LaneDetector()
#print("Lane Detector Initiation Success")
self.lanePlotter = LanePlotter()
def start():
global pub
rospy.init_node('my_driver')
pub = rospy.Publisher('xycar_motor_msg', Int32MultiArray, queue_size=1)
marker_publisher = rospy.Publisher('visualization_marker', Marker, queue_size=5)
capture = cv2.VideoCapture("/home/jjong/catkin_ws/src/xycar_simul/src/track-s.mkv")
rate = rospy.Rate(60)
Speed = 20
current = 0
while True:
ret , img = capture.read()
if ret == False:
break
now = time.time()
bev = BirdEyeView(img)
ldt = LaneDetector(bev)
Binary_img = image_processing(bev, img)
info = ldt.slidingWindows(Binary_img)
final_frame, left_curverad, right_curverad = ldt.drawFitLane(img, Binary_img, info)
left_curvature, right_curvature= 1/left_curverad, 1/right_curverad
if info['valid_left_line'] & info['valid_right_line'] :
final_curvature = WEIGHT*(left_curvature + right_curvature)/2
elif info['valid_left_line'] :
final_curvature = WEIGHT*left_curvature
elif info['valid_right_line'] :
final_curvature = WEIGHT*right_curvature
if final_curvature >50 :
final_curvature = 50
elif final_curvature < -50 :
final_curvature = -50
#cv2.imshow("roi_frame", roi_frame)
cv2.imshow("Binary_img", Binary_img)
#cv2.imshow("warped_frame2",warped_frame2)
#문자 출력
cv2.putText(final_frame, "Radius of curvature : " + str(final_curvature), (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
# cv2.imshow('image',final_frame)
if cv2.waitKey(100) > 0 : break
print("time : ", now - current)
print("=============================")
current = time.time()
show_text_in_rviz(marker_publisher, "1/curvature : " + str(int(final_curvature)))
pub_motor((final_curvature), Speed)
rate.sleep()
class LaneTracker():
def __init__(self):
self.undistorter = Undistorter()
#print("Distortion Initiation Success")
self.thresholder = Thresholder()
#print("Thresholding Initiation Success")
self.ptransformer = PerspectiveTransformer()
#print("Perspective Transformer Initiation Success")
self.laneDetector = LaneDetector()
#print("Lane Detector Initiation Success")
self.lanePlotter = LanePlotter()
#print("Lane Plotter Initiation Success")
def process(self, img):
#Undistort the image to remove camera and lens distortion
undist_img = self.undistorter.undistort(img)
#Threshold the image to better identify lane lines
thresh_img = self.thresholder.threshold(undist_img)
#Perform perspective transformation so that converging lines become parallel without perspective error
warp_img = self.ptransformer.warp(thresh_img)
#Detect the lines through curve fit and curve-fit co-efficients
left_fit, right_fit = self.laneDetector.detect(warp_img)
#Get left lane and right lane curvature, car offset
left_curverad, right_curverad = self.laneDetector.get_curvature(
warp_img)
car_offset = self.laneDetector.get_car_offset(warp_img)
#Plot the detected lanes on image
plotted_img = self.lanePlotter.plotPolygon(img, left_fit, right_fit,
self.ptransformer.Minv)
left_curve_str = "Left ROC: " + str(round(left_curverad, 0)) + "m"
right_curve_str = "Right ROC: " + str(round(right_curverad, 0)) + "m"
car_offset_str = "Car Offset:" + str(round(car_offset, 3)) + "m"
#Overlay text
text_img = self.lanePlotter.textOverLay(plotted_img,
left_curve_str,
pos=(100, 100))
text_img = self.lanePlotter.textOverLay(text_img,
right_curve_str,
pos=(100, 150))
text_img = self.lanePlotter.textOverLay(text_img,
car_offset_str,
pos=(100, 200))
return plotted_img
def Pipeline(image):
# Initialize perspective transformer
perspectiveTransformer = PerspectiveTransformer(
np.float32(
[src_top_right, src_bottom_right, src_bottom_left, src_top_left]),
np.float32(
[dst_top_right, dst_bottom_right, dst_bottom_left, dst_top_left]))
# Initialize image thresholder
thresholder = ImageThresholder()
# Initialize histogram
laneDetector = LaneDetector()
#Perform image undistortion
undistorted_image = UndistortImage(image, cameraMatrix, distCoeffs)
#Perform perspective transform
perspective_transformed = perspectiveTransformer.TransformImage(
undistorted_image)
#Perform thresholding
thresholded_image, _, _, _, _, _, _ = thresholder.Perform(
perspective_transformed)
#Calculate the histogram
centroids = laneDetector.FindWindowCentroids(thresholded_image)
centroids_image = laneDetector.DrawWindowCentroids(thresholded_image,
centroids)
left_poly, right_poly = laneDetector.FitAPolyline(thresholded_image,
centroids)
poly_region_image = laneDetector.DrawPolylines(undistorted_image,
left_poly, right_poly)
radius_left = laneDetector.FindRadiusOfCurvature(left_poly, 720)
radius_right = laneDetector.FindRadiusOfCurvature(right_poly, 720)
curvature = (radius_left + radius_right) / 2.0
center = laneDetector.CarPosition(left_poly, right_poly)
#Apply inverse transform on lane region
lane_detector_region = perspectiveTransformer.InverseTransformImage(
poly_region_image)
pipeline_output = cv2.addWeighted(undistorted_image, 1,
lane_detector_region, 0.3, 0)
return pipeline_output, undistorted_image, thresholded_image, perspective_transformed, centroids_image, poly_region_image, lane_detector_region, curvature, center
def __init__(self):
self.bridge = CvBridge()
""" Set up all needed params """
height = 480
width = 640
h = 1.518 # meters
t = np.asarray([0, 0, -h], np.float32)
# Map from world frame to camera frame
pitch = np.deg2rad(5) # positive tilts down
R = np.asarray([[0, -1, 0],
[np.sin(pitch), 0, -np.cos(pitch)],
[np.cos(pitch), 0, np.sin(pitch)]], np.float32)
K = np.asarray([[617.2716, 0, 327.2818],
[0, 617.1263, 245.0939],
[0, 0, 1]], np.float32)
D = np.asarray([0, 0, 0, 0, 0], np.float32)
FOV_h = np.radians(91.2)
FOV_v = np.radians(65.5)
params = CameraParams()
params.K = K
params.D = D
params.FOV_h = FOV_h
params.FOV_v = FOV_v
params.height = height
params.width = width
self.Detector = LaneDetector(R, t, params)
# TODO: Configure via json
self.image_sub = rospy.Subscriber("camera/color/image_raw", Image, self.callback)
self.visualization_pub = rospy.Publisher("lane_detector/visualization", Image)
self.hsv_pub = rospy.Publisher("lane_detector/color_threshold", Image)
self.grad_pub = rospy.Publisher("lane_detector/gradient_threshold", Image)
self.nav_pub = rospy.Publisher("lane_detector/waypoints", Path)
def __init__(self):
self.bridge = CvBridge()
""" Set up all needed params """
height = 480
width = 640
h = 1.518 # meters
t = np.asarray([0, 0, -h], np.float32)
# Map from world frame to camera frame
R = np.asarray([[0, -1, 0], [0, 0, -1], [1, 0, 0]], np.float32)
K = np.asarray(
[[617.2716, 0, 327.2818], [0, 617.1263, 245.0939], [0, 0, 1]],
np.float32)
D = np.asarray([0, 0, 0, 0, 0], np.float32)
FOV_h = np.radians(91.2)
FOV_v = np.radians(65.5)
params = CameraParams()
params.K = K
params.D = D
params.FOV_h = FOV_h
params.FOV_v = FOV_v
params.height = height
params.width = width
self.Detector = LaneDetector(R, t, params)
# TODO: Configure via json
self.image_sub = rospy.Subscriber("camera/color/image_raw", Image,
self.callback)
# publish coefficients of spline as array
#self.spline_pub = rospy.Publisher("lane_splines", Float32MultiArray)
# publish top down view for visualization
self.visualization_pub = rospy.Publisher("lane_detector/visualization",
Image)
self.mask_pub = rospy.Publisher("lane_detector/mask", Image)
self.nav_pub = rospy.Publisher("lane_detector/waypoints", Path)
def process_video(infile, outfile, camera=None, vehicle=None):
'''
Load video, process individual image frames, output frames to new Video
Parameters:
infile Filename of input video
outfile Filename of output video
camera Filename of Camera Calibration pickle (from calibration.py)
vehicle Filename of Vehicle Model pickle (from train.py)
'''
print("Reading {}".format(os.path.basename(infile)))
vehicle_detection = VehicleDetection(from_pickle=vehicle)
lane_detector = LaneDetector(use_smoothing=True, camera=camera)
multiple_detector = MultipleDetector(vehicle_detection=vehicle_detection)
clip = VideoFileClip(infile)
adj_clip = clip.fl_image(multiple_detector.process_image)
adj_clip.write_videofile(outfile, audio=False)
class lane_detector:
def __init__(self):
self.bridge = CvBridge()
""" Set up all needed params """
height = 480
width = 640
h = 1.518 # meters
t = np.asarray([0, 0, -h], np.float32)
# Map from world frame to camera frame
R = np.asarray([[0, -1, 0], [0, 0, -1], [1, 0, 0]], np.float32)
K = np.asarray(
[[617.2716, 0, 327.2818], [0, 617.1263, 245.0939], [0, 0, 1]],
np.float32)
D = np.asarray([0, 0, 0, 0, 0], np.float32)
FOV_h = np.radians(91.2)
FOV_v = np.radians(65.5)
params = CameraParams()
params.K = K
params.D = D
params.FOV_h = FOV_h
params.FOV_v = FOV_v
params.height = height
params.width = width
self.Detector = LaneDetector(R, t, params)
# TODO: Configure via json
self.image_sub = rospy.Subscriber("camera/color/image_raw", Image,
self.callback)
# publish coefficients of spline as array
#self.spline_pub = rospy.Publisher("lane_splines", Float32MultiArray)
# publish top down view for visualization
self.visualization_pub = rospy.Publisher("lane_detector/visualization",
Image)
self.mask_pub = rospy.Publisher("lane_detector/mask", Image)
self.nav_pub = rospy.Publisher("lane_detector/waypoints", Path)
def callback(self, data):
cv_img = self.bridge.imgmsg_to_cv2(data, "rgb8")
# Run pipeline
mask_img = self.Detector.filter(cv_img)
blur_img = self.Detector.blur_mask(mask_img)
warped_image = self.Detector.perspective_warp(blur_img)
try:
(left, center, right) = self.Detector.sliding_window(warped_image)
waypoints = self.Detector.generate_waypoints(cv_img, center)
# Generate publishing stuff
lane_image = self.Detector.draw_lanes(cv_img, left, right)
path = Path()
path.header = data.header
num_points = waypoints.shape[1]
for i in range(num_points):
x = float(waypoints[0, i])
y = float(waypoints[1, i])
theta = waypoints[2, i]
w = np.cos(theta / 2)
z = np.sin(theta / 2)
pose = PoseStamped()
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = 0
p.orientation.x = 0.0
p.orientation.y = 0.0
p.orientation.z = z
p.orientation.w = w
pose.pose = p
"""
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientaion.z = 0
pose.pose.orientation.w = 1
"""
pose.header = data.header
path.poses.append(pose)
self.nav_pub.publish(path)
self.visualization_pub.publish(
self.bridge.cv2_to_imgmsg(lane_image, 'rgb8'))
except:
print("Failed to generate path")
rospy.logerr("LOLNO")
# Publish messages
self.mask_pub.publish(self.bridge.cv2_to_imgmsg(warped_image, 'mono8'))
# print("Distortion Initiation Success")
# from Thresholder import Thresholder
# thresholder = Thresholder()
# print("Thresholding Initiation Success")
# from PerspectiveTransformer import PerspectiveTransformer
# ptransformer = PerspectiveTransformer()
# print("Perspective Transformer Initiation Success")
# from LaneDetector import LaneDetector
# laneDetector = LaneDetector()
# print("Lane Detector Initiation Success")
# from LanePlotter import LanePlotter
# lanePlotter = LanePlotter()
# print("Lane Plotter Initiation Success")
import cv2
import numpy as np
t = cv2.imread('warped.jpg')
img = cv2.cvtColor(t, cv2.COLOR_BGR2GRAY)
thresh_img = np.zeros_like(img)
thresh_img[img > 127] = 1
from LaneDetector import LaneDetector
laneDetector = LaneDetector()
print("Lane Detector Initiation Success")
left_fit, right_fit = laneDetector.detect(thresh_img)
cv2.imwrite('detect.jpg', detect_img)
FRAME_SHAPE = (1280, 720)
HIST_STEPS = 10
OFFSET = 250
FRAME_MEMORY = 7
SRC = np.float32([(132, 703), (540, 466), (740, 466), (1147, 703)])
DST = np.float32([(SRC[0][0] + OFFSET, 720), (SRC[0][0] + OFFSET, 0),
(SRC[-1][0] - OFFSET, 0), (SRC[-1][0] - OFFSET, 720)])
VIDEOS = [
"../videos/project_video.mp4", "../videos/challenge_video.mp4",
"../videos/harder_challenge_video.mp4"
]
SELECTED_VIDEO = 0
if __name__ == '__main__':
cam_calibration = get_camera_calibration()
cam_calibrator = CameraCalibrator(FRAME_SHAPE, cam_calibration)
ld = LaneDetector(SRC,
DST,
n_frames=FRAME_MEMORY,
cam_calibration=cam_calibrator,
transform_offset=OFFSET)
clip1 = VideoFileClip(VIDEOS[SELECTED_VIDEO])
project_clip = clip1.fl_image(ld.process_frame)
project_output = VIDEOS[SELECTED_VIDEO][:-4] + '_ann.mp4'
project_clip.write_videofile(project_output, audio=False)
class lane_detector:
def __init__(self):
self.bridge = CvBridge()
""" Set up all needed params """
height = 480
width = 640
h = 1.518 # meters
t = np.asarray([0, 0, -h], np.float32)
# Map from world frame to camera frame
pitch = np.deg2rad(5) # positive tilts down
R = np.asarray([[0, -1, 0],
[np.sin(pitch), 0, -np.cos(pitch)],
[np.cos(pitch), 0, np.sin(pitch)]], np.float32)
K = np.asarray([[617.2716, 0, 327.2818],
[0, 617.1263, 245.0939],
[0, 0, 1]], np.float32)
D = np.asarray([0, 0, 0, 0, 0], np.float32)
FOV_h = np.radians(91.2)
FOV_v = np.radians(65.5)
params = CameraParams()
params.K = K
params.D = D
params.FOV_h = FOV_h
params.FOV_v = FOV_v
params.height = height
params.width = width
self.Detector = LaneDetector(R, t, params)
# TODO: Configure via json
self.image_sub = rospy.Subscriber("camera/color/image_raw", Image, self.callback)
self.visualization_pub = rospy.Publisher("lane_detector/visualization", Image)
self.hsv_pub = rospy.Publisher("lane_detector/color_threshold", Image)
self.grad_pub = rospy.Publisher("lane_detector/gradient_threshold", Image)
self.nav_pub = rospy.Publisher("lane_detector/waypoints", Path)
def callback(self, data):
cv_img = self.bridge.imgmsg_to_cv2(data, "rgb8")
# Run pipeline
hsv_mask = self.Detector.color_threshold(cv_img)
grad_mask = self.Detector.gradient_threshold(cv_img)
mask_img = cv.bitwise_or(hsv_mask, grad_mask)
blur_img = self.Detector.blur_mask(mask_img)
warped_image = self.Detector.perspective_warp(blur_img)
try:
(left, center, right) = self.Detector.sliding_window(warped_image)
waypoints = self.Detector.generate_waypoints(cv_img, center)
# Generate publishing stuff
lane_image = self.Detector.draw_lanes(cv_img, left, right)
path = Path()
path.header = data.header
num_points = waypoints.shape[1]
for i in range(num_points):
x = float(waypoints[0,i])
y = float(waypoints[1,i])
theta = waypoints[2,i]
w = np.cos(theta/2)
z = np.sin(theta/2)
pose = PoseStamped()
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = 0
p.orientation.x = 0.0
p.orientation.y = 0.0
p.orientation.z = z
p.orientation.w = w
pose.pose = p
pose.header = data.header
path.poses.append(pose)
self.nav_pub.publish(path)
self.visualization_pub.publish(self.bridge.cv2_to_imgmsg(lane_image, 'rgb8'))
except:
y2 = line[3]
cv2.line(img, (int(x1), int(y1)), (int(x2), int(y2)), color, thickness)
def draw_lane(img, lane):
if lane.left_line:
draw_line(img, lane.left_line.line_vector, [0, 0, 200], 3)
if lane.right_line:
draw_line(img, lane.right_line.line_vector, [0, 0, 200], 3)
#reading in an image
image = mpimg.imread('test_images/solidYellowLeft.jpg')
# get image dimensions and make internal copy
lane_detector = LaneDetector()
lane_detector.set_image(image)
lane_detector.find_lanes()
output = lane_detector.lanes_img
#plt.imshow(output, cmap='gray')
overlay = np.zeros_like(image)
draw_lane(overlay, lane_detector.lane)
img = cv2.cvtColor(cv2.addWeighted(image, 1., overlay, 0.9, 0.),
cv2.COLOR_RGB2BGR)
cv2.imwrite('test_images/out.png', img)
plt.imshow(output)
plt.show()
import numpy as np
import cv2
import matplotlib.pyplot as plt
from BirdEyeView import BirdEyeView
from LaneDetector import LaneDetector
from CameraCalibrator import Calibrator
##초기화 단계, 차선 인지에 필요한 변수들을 계산함
bev = BirdEyeView() ## 버드아이뷰로 전환할 때 사용할 부분
cbrt = Calibrator()
cbrt.setVariables(
) ## 카메라 칼리브레이션을 위한 메트릭스를 세팅함. 매 프레임 새로 구하면 너무 느려서 한번 구해놓고 쓰게 만듦
ldt = LaneDetector(bev) ## 차선인식 클래스
cap = cv2.VideoCapture('hard_video.mp4')
if not cap.isOpened():
print('cap is not opened')
exit(-1)
while True:
ret, frame = cap.read()
#print(frame.shape)
if not ret:
continue
u_frame = cbrt.getUndistortedImg(frame)
roi_frame = u_frame.copy()
roi_frame = bev.setROI(roi_frame)
warped_frame = bev.warpPerspect(u_frame)
color_comb_frame = bev.combineAllColorChannel(warped_frame, [150, 255],
[20, 100])
cv2.line(img, (int(x1), int(y1)), (int(x2), int(y2)), color, thickness)
def draw_lane(img, lane):
if lane.left_line:
draw_line(img, lane.left_line.line_vector, [0, 0, 200], 3)
if lane.right_line:
draw_line(img, lane.right_line.line_vector, [0, 0, 200], 3)
#reading video clip
#clip = cv2.VideoCapture('test_videos/solidWhiteRight.mp4')
#clip = cv2.VideoCapture('test_videos/solidYellowLeft.mp4')
clip = cv2.VideoCapture('test_videos/challenge.mp4')
lane_detector = LaneDetector()
lane_detector.quiet()
fourcc = cv2.VideoWriter_fourcc(*'DIVX')
out = cv2.VideoWriter('test_videos/out.avi', fourcc, 20.0, (960, 540))
while (clip.isOpened()):
ret, frame = clip.read()
if frame is not None:
lane_detector.set_image(frame)
lane_detector.find_lanes()
overlay = np.zeros_like(frame)
draw_lane(overlay, lane_detector.lane)
if args.t1 == None:
if args.video_file == "project":
args.t1 = 20 * frame_rate
else:
args.t1 = 0
else:
t_array = args.t1.split(".")
args.t1 = int(t_array[0]) * frame_rate
if len(t_array) == 2:
args.t1 += int(t_array[1])
args.video_file += "_video.mp4"
pipeline = YUVPipeline()
detector = LaneDetector(pipeline)
def process_frame(frame):
global counter
if not args.render:
pipeline.poll()
detector.process(frame)
detector.annotate(frame)
if args.render and not args.annotate:
new_frame = detector.annotated_frame
else:
new_frame = np.zeros((frame.shape[0],frame.shape[1]//8*9,3),np.uint8)
scale_and_paste(new_frame, detector.annotated_frame, (0,0), factor=0.75)