def main(): global theta_div cap = cv2.VideoCapture(0) ticks = 0 lt = track.LaneTracker(2, 0.1, 150) ld = detect.LaneDetector(120) while cap.isOpened(): prec_tick = ticks ticks = cv2.getTickCount() dt = (ticks - prec_tick) / cv2.getTickFrequency() ret, frame = cap.read() predicted = lt.predict(dt) lanes = ld.detect(frame) if predicted is not None: cv2.line(frame, (predicted[0][0], predicted[0][1]), (predicted[0][2], predicted[0][3]), (0, 0, 255), 5) cv2.line(frame, (predicted[1][0], predicted[1][1]), (predicted[1][2], predicted[1][3]), (0, 0, 255), 5) theta1 = np.arctan2((predicted[0][3] - predicted[0][1]), (predicted[0][2] - predicted[0][0])) theta2 = np.arctan2((predicted[1][3] - predicted[1][1]), (predicted[1][2] - predicted[1][0])) div = (np.pi - np.abs(theta1) - np.abs(theta2)) / 2 theta_div = (np.pi - np.abs(theta1) - div) - np.pi / 2 lt.update(lanes) cv2.imshow('', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break
def main(self): # cap = cv2.VideoCapture(0) cap = cv2.VideoCapture('project_video.mp4') ticks = 0 lt = track.LaneTracker(2, 0.1, 500) ld = detect.LaneDetector(180) while cap.isOpened(): precTick = ticks ticks = cv2.getTickCount() dt = (ticks - precTick) / cv2.getTickFrequency() ret, frame = cap.read() predicted = lt.predict(dt) lanes = ld.detect(frame) if predicted is not None: cv2.line(frame, (predicted[0][0], predicted[0][1]), (predicted[0][2], predicted[0][3]), (0, 0, 255), 7) cv2.line(frame, (predicted[1][2], predicted[1][3]), (predicted[1][0], predicted[1][1]), (0, 0, 255), 7) center_coordinates_top = ( (abs(predicted[1][0] - predicted[0][2]) / 2) + predicted[0][2], predicted[1][1]) center_coordinates_bottom = ( (abs(predicted[0][0] - predicted[1][2]) / 2) + predicted[0][0], predicted[0][1]) cv2.line(frame, center_coordinates_top, center_coordinates_bottom, (0, 2555, 0), 7) error = frame.shape[1] / 2 - ( abs(predicted[0][0] - predicted[1][2]) / 2 + predicted[0][0]) a = [0, 352] b = [ center_coordinates_bottom[0], center_coordinates_bottom[1] ] c = [center_coordinates_top[0], center_coordinates_top[1]] ba = np.subtract(a, b) bc = np.subtract(c, b) cosine_angle = np.dot( ba, bc) / (np.linalg.norm(ba) * np.linalg.norm(bc)) angle = 90. - np.degrees(np.arccos(cosine_angle)) cv2.putText(frame, 'Crosstrack Error:' + str('%.2f' % error), (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) cv2.putText(frame, 'Angle:' + str(angle[0]), (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) lt.update(lanes) try: self.lane_image_pub.publish( self.bridge.cv2_to_imgmsg(frame, "bgr8")) except CvBridgeError as e: print(e)
def main(): images = [ cv2.imread(file) for file in glob.glob("S:\CNN\Matlab\data/*.png") ] ticks = 0 lt = track.LaneTracker(2, 0.1, 500) ld = detect.LaneDetector(180) for frame in images: precTick = ticks ticks = cv2.getTickCount() dt = (ticks - precTick) / cv2.getTickFrequency() ## ret, frame = cap.read() predicted = lt.predict(dt) lanes = ld.detect(frame) if predicted is not None: cv2.line(frame, (predicted[0][0], predicted[0][1]), (predicted[0][2], predicted[0][3]), (0, 0, 255), 5) cv2.line(frame, (predicted[1][0], predicted[1][1]), (predicted[1][2], predicted[1][3]), (0, 0, 255), 5) lt.update(lanes) cv2.imshow('', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break
def main(video_path): cap = cv2.VideoCapture(video_path) ticks = 0 lt = track.LaneTracker(2, 0.1, 500) ld = detect.LaneDetector(180) while cap.isOpened(): precTick = ticks ticks = cv2.getTickCount() dt = (ticks - precTick) / cv2.getTickFrequency() ret, frame = cap.read() predicted = lt.predict(dt) lanes = ld.detect(frame) if predicted is not None: cv2.line(frame, (predicted[0][0], predicted[0][1]), (predicted[0][2], predicted[0][3]), (0, 0, 255), 5) cv2.line(frame, (predicted[1][0], predicted[1][1]), (predicted[1][2], predicted[1][3]), (0, 0, 255), 5) lt.update(lanes) cv2.imshow('', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break
def main(video_path): cap = cv2.VideoCapture(video_path) ticks = 0 lt = track.LaneTracker(2, 0.1, 500) ld = detect.LaneDetector(0) while cap.isOpened(): precTick = ticks ticks = cv2.getTickCount() dt = (ticks - precTick) / cv2.getTickFrequency() dt *= 0.2 ret, frame = cap.read() frame = cv2.resize(frame, (1280, 720), interpolation=cv2.INTER_CUBIC) time1 = time.time() predicted = lt.predict(dt) lanes, frame2 = ld.detect(frame) print time.time() - time1 imshape = frame.shape vertices = np.array([[(.55 * imshape[1], 0.67 * imshape[0]), (imshape[1], imshape[0]), (0, imshape[0]), (.45 * imshape[1], 0.67 * imshape[0])]], dtype=np.int32) cv2.polylines(frame, [vertices], True, (0, 255, 0), 3) predicted2 = [] if lanes.count(None) == 0: #print lanes predicted2.append(map(int, list(lanes[0]))) predicted2.append(map(int, list(lanes[1]))) else: predicted2 = None if predicted is not None: cv2.line(frame, (int(predicted[0][0]), int(predicted[0][1])), (int(predicted[0][2]), int(predicted[0][3])), (0, 0, 255), 5) cv2.line(frame, (int(predicted[1][0]), int(predicted[1][1])), (int(predicted[1][2]), int(predicted[1][3])), (0, 0, 255), 5) ''' if predicted2 is not None: cv2.line(frame, (int(predicted2[0][0]), int(predicted2[0][1])), (int(predicted2[0][2]), int(predicted2[0][3])), (255, 0, 0), 5) cv2.line(frame, (int(predicted2[1][0]), int(predicted2[1][1])), (int(predicted2[1][2]), int(predicted2[1][3])), (255, 0, 0), 5) ''' lt.update(lanes) cv2.imshow('', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break
def main(): global pos pos = 0 # t1 = threading.Thread(target=camera_thread) t2 = threading.Thread(target=pid_thread) # t1.start() t2.start() # t1.join() cap = cv2.VideoCapture(0) ticks = 0 lt = track.LaneTracker(2, 0.1, 150) ld = detect.LaneDetector(120) while cap.isOpened(): prec_tick = ticks ticks = cv2.getTickCount() dt = (ticks - prec_tick) / cv2.getTickFrequency() ret, frame = cap.read() predicted = lt.predict(dt) lanes = ld.detect(frame) if predicted is not None: cv2.line(frame, (predicted[0][0], predicted[0][1]), (predicted[0][2], predicted[0][3]), (0, 0, 255), 5) cv2.line(frame, (predicted[1][0], predicted[1][1]), (predicted[1][2], predicted[1][3]), (0, 0, 255), 5) theta1 = np.arctan2((predicted[0][3] - predicted[0][1]), (predicted[0][2] - predicted[0][0])) theta2 = np.arctan2((predicted[1][3] - predicted[1][1]), (predicted[1][2] - predicted[1][0])) div = (np.pi - np.abs(theta1) - np.abs(theta2)) / 2 lock.acquire() try: pos = ((np.pi - np.abs(theta1) - div) - np.pi / 2) * 50 / np.pi finally: lock.release() lt.update(lanes) cv2.imshow('', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break t2.join()
def main(input_vidoe_path): # input video cap = cv2.VideoCapture(input_vidoe_path) # initialize detector and tracker here lt = track.LaneTracker(2, 0.1, 500) ld = detect.LaneDetector(180) # initialize time tick ticks = 0 while cap.isOpened(): # read frame ret, frame = cap.read() # start count tick precTick = ticks ticks = cv2.getTickCount() dt = (ticks - precTick) / cv2.getTickFrequency() # start predict and detect lane # Kalman filter predict step predicted = lt.predict(dt) lanes = ld.detect(frame) if predicted is not None: cv2.line(frame, (predicted[0][0], predicted[0][1]), (predicted[0][2], predicted[0][3]), (0, 0, 255), 5) cv2.line(frame, (predicted[1][0], predicted[1][1]), (predicted[1][2], predicted[1][3]), (0, 0, 255), 5) # Kalman filter update step lt.update(lanes) cv2.imshow('', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break # When everything done, release the capture cap.release() cv2.destroyAllWindows()
def main(video_path): cap = cv2.VideoCapture(video_path) ret, img = cap.read() height, width, layer = img.shape print height, width, layer screen_rate = 0.4 width = int(width * screen_rate) height = int(height * screen_rate) ticks = 0 lt = track.LaneTracker(2, 0.1, 500) ld = detect.LaneDetector(180) while cap.isOpened(): precTick = ticks ticks = cv2.getTickCount() dt = (ticks - precTick) / cv2.getTickFrequency() ret, frame = cap.read() frame = cv2.resize(frame, None, fx=screen_rate, fy=screen_rate, interpolation=cv2.INTER_CUBIC) predicted = lt.predict(dt) lanes = ld.detect(frame) #goruntu ortasi cv2.line(frame, (int(width / 2), 0), (int(width / 2), height), (255, 255, 255), 2) if predicted is not None: Ax = predicted[0][2] Ay = predicted[0][3] Bx = predicted[0][0] By = predicted[0][1] Cx = predicted[1][2] Cy = predicted[1][3] Dx = predicted[1][0] Dy = predicted[1][1] #koseler cv2.line(frame, (Ax, Ay), (Bx, By), (0, 255, 255), 5) cv2.line(frame, (Cx, Cy), (Dx, Dy), (0, 255, 255), 5) #alan ic = np.array([[Ax, Ay], [Bx, By], [Cx, Cy], [Dx, Dy]], np.int32) #alanin ortasi cv2.line(frame, ((Ax + Dx) / 2, (Ay + Dy) / 2), ((Bx + Cx) / 2, (By + Cy) / 2), (0, 0, 255), 5) cv2.circle(frame, ((Ax + Dx) / 2, (Ay + Dy) / 2), 5, (255, 0, 0), -1) cv2.line(frame, ((Ax + Dx) / 2, (Ay + Dy) / 2), (int(width / 2), (Ay + Dy) / 2), (0, 255, 0), 3) veri = "s" + str(((Ax + Dx) / 2) - (width / 2)) cv2.putText(frame, veri, ((Ax + Dx) / 2, (Ay + Dy) / 2), font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) #kose isimleri cv2.putText(frame, 'A', (Ax, Ay), font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) cv2.putText(frame, 'B', (Bx, By), font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) cv2.putText(frame, 'C', (Cx, Cy), font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) cv2.putText(frame, 'D', (Dx, Dy), font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) overlay = frame.copy() cv2.fillPoly(overlay, [ic], (100, 50, 50, 50)) opacity = 0.4 cv2.addWeighted(overlay, opacity, frame, 1 - opacity, 0, frame) lt.update(lanes) cv2.imshow('output', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break
from __future__ import division import cv2 import numpy as np import track import detect import time from grabscreen import grab_screen ticks = 0 road_horizon=320 lt = track.LaneTracker() vertices = np.array([[0, 256], [0, 220], [220, 150], [292, 150], [512, 200], [512, 256]], np.int32) ld = detect.LaneDetector(vertices,road_horizon) #horizon line y coordinate with open("Output.txt", "w") as text_file: while True: begin=time.time() precTick = ticks ticks = cv2.getTickCount() dt = (ticks - precTick) / cv2.getTickFrequency() frame = np.array(grab_screen(region=(10, 40, 810, 640))) frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) #frame = cv2.imread('C:/Users/shun.yang/Desktop/lane_detect/track/test.png') #frame = cv2.imread('C:/Users/shun.yang/Desktop/lane_detect/lane_alex/img/1.png') predicted = lt.predict(dt) lanes = ld.detect(frame) if predicted is not None: cv2.line(frame, (predicted[0][0], predicted[0][1]), (predicted[0][2], predicted[0][3]), (0, 0, 255), 5)
def main(video_path): # cap = cv2.VideoCapture(0) cap = cv2.VideoCapture('project_video.mp4') ticks = 0 lt = track.LaneTracker(2, 0.1, 500) ld = detect.LaneDetector(180) while cap.isOpened(): precTick = ticks ticks = cv2.getTickCount() dt = (ticks - precTick) / cv2.getTickFrequency() ret, frame = cap.read() # frame = cv2.flip( frame, -1 ) # pts1 = np.float32([[0, 0], [640, 0], [640, 352], [0, 352]]) # pts2 = np.float32([[300, 176], [12, 174], [620, 344], [72, 354]]) # matrix = cv2.getPerspectiveTransform(pts1,pts2) # frame = cv2.warpPerspective(frame, matrix, (500, 500)) predicted = lt.predict(dt) lanes = ld.detect(frame) if predicted is not None: cv2.line(frame, (predicted[0][0], predicted[0][1]), (predicted[0][2], predicted[0][3]), (0, 0, 255), 7) cv2.line(frame, (predicted[1][2], predicted[1][3]), (predicted[1][0], predicted[1][1]), (0, 0, 255), 7) center_coordinates_top = ( (abs(predicted[1][0] - predicted[0][2]) / 2) + predicted[0][2], predicted[1][1]) center_coordinates_bottom = ( (abs(predicted[0][0] - predicted[1][2]) / 2) + predicted[0][0], predicted[0][1]) cv2.line(frame, center_coordinates_top, center_coordinates_bottom, (0, 2555, 0), 7) cv2.circle(frame, (int(frame.shape[1] / 2), int(frame.shape[0])), 10, (255, 0, 0), 2) # Image Center marker # cv2.circle(frame, center_coordinates_bottom, 20, (0, 255, 0) , 4) # Center bottom marker # cv2.circle(frame, center_coordinates_top, 20, (255, 0, 0) , 4) # Center top marker # cv2.circle(frame, (predicted[0][2], predicted[0][3]), 20, (0, 255, 0) , 4) # Left lane marker # cv2.circle(frame, (predicted[1][0], predicted[1][1]), 20, (0, 255, 0) , 4) # Right lane marker # print('Lane left:',predicted[0][2], predicted[0][3], 'Lane right:',predicted[1][0], predicted[1][1] # ,'Lane center:',center_coordinates_top[0] ) error = frame.shape[1] / 2 - ( abs(predicted[0][0] - predicted[1][2]) / 2 + predicted[0][0]) angle = np.degrees( np.arctan(center_coordinates_top[0], center_coordinates_top[1])) a = np.array([0, 352]) b = np.array( [center_coordinates_bottom[0], center_coordinates_bottom[1]]) c = np.array( [center_coordinates_top[0], center_coordinates_top[1]]) ba = a - b bc = c - b cosine_angle = np.dot( ba, bc) / (np.linalg.norm(ba) * np.linalg.norm(bc)) angle = 90. - np.degrees(np.arccos(cosine_angle)) print('Crosstrack Error:', error) cv2.putText(frame, 'Crosstrack Error:' + str('%.2f' % error), (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) cv2.putText(frame, 'Angle:' + str(angle[0]), (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) lt.update(lanes) cv2.imshow('', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break
def main(): cap = cv2.VideoCapture(0) # 0 cv2.namedWindow("demo") # a new window ticks = 0 s = 1 t = 0 lt = track.LaneTracker(2, 0.1, 200) ld = detect.LaneDetector(upline, lolane, Xl, Xr) ip_port = ('192.168.1.176', 50002) sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind(('0.0.0.0', 50000)) #UDP服务器端口和IP绑定 #buf, addr = sock.recvfrom(40960) sock1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock1.bind(('0.0.0.0', 50001)) #UDP服务器端口和IP绑定 #buf1, addr1 = sock1.recvfrom(40960) sock2 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock2.bind(('0.0.0.0', 50002)) #UDP服务器端口和IP绑定 #buf2, addr2 = sock2.recvfrom(40960) dn.set_gpu(1) net = dn.load_net(b"cfg/yolov3.cfg", b"yolov3.weights", 0) meta = dn.load_meta(b"cfg/coco.data") datad = 0.01 pres = 0 ns = 0 speed = 0 ndis = 0 while (1): precTick = ticks ticks = cv2.getTickCount() dt = (ticks - precTick) / cv2.getTickFrequency() # print(dt) ret, frame = cap.read() # object detection using yolo res = dn.detect_numpy(net, meta, image=frame) ndis = 0 for item in res: pt1 = (int(item[2][0] - item[2][2] * 0.5), int(item[2][1] - item[2][3] * 0.5)) pt2 = (int(item[2][0] + item[2][2] * 0.5), int(item[2][1] + item[2][3] * 0.5)) cv2.rectangle(frame, pt1, pt2, (0, 255, 0), 4) cv2.putText(frame, str(item[0], encoding="utf8"), pt1, cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2) #cv2.rectangle(frame, pt1, pt2, (0, 0, 255), 4) ns = item[2][3] ndis = max(pt1[1], pt2[1]) speed = ns - pres pres = ns print('speed:', speed) print('ndis:', ndis) # cv2.rectangle(frame, (0,0), (599, 402), (0, 255, 0), 4) # frame2 = frame[upline:640, 0:480] if (ret): lanes = ld.detect(frame) # print(lanes) # if lanes: predicted = lt.predict(dt) if lanes and (t > 1): # predict succeed sys.exit() # if s: # print(ld.detectLight(frame)) t = t + 1 if predicted is not None and (predicted[0][1][0] == predicted[0][1][0]): if (t > 0): t = t - 1 xa, ya, xb, yb = predicted[0] x1, y1, x2, y2 = predicted[1] print('1: ', xa, ' ', ya) print('2: ', xb, ' ', yb) print('3: ', x1, ' ', y1) print('4: ', x2, ' ', y2) pt1 = [138, 480] pt2 = [265, 380] pt3 = [550, 480] pt4 = [370, 380] ptc = np.float32([pt1, pt2, pt3, pt4]) ptt1 = [200, 450] ptt2 = [200, 300] ptt3 = [500, 450] ptt4 = [500, 300] pttc = np.float32([ptt1, ptt2, ptt3, ptt4]) matrix = cv2.getPerspectiveTransform(ptc, pttc) print('matrix:', matrix) nimg = cv2.warpPerspective(frame, matrix, (650, 500)) if float(xb - xa) == 0: kl = -slopeMax else: kl = float(yb - ya) / float(xb - xa) if float(x2 - x1) == 0: k1 = slopeMax else: k1 = float(y2 - y1) / float(x2 - x1) X = (xb * x2 - x1 * xa) / (xb + x2 - x1 - xa) # print("x") # print(xb, X, x1) #left m right -> X # data = X + 320 data = (xb + x2) / 2 # data = drstrdrv(data,) fYaw = 300 - 5 - data[0] if (abs(fYaw) < 30): fYaw = fYaw * 0.2 elif (abs(fYaw) < 60): fYaw = fYaw * 0.8 else: fYaw = fYaw * 0.9 datac = float(math.atan(fYaw / 100)) / 2 if (datad >= 0.25): datad = 0.08 elif (abs(fYaw) > 23): datad = 0.25 + (abs(fYaw) / 120.0) # data = drstrdrv(data,speed) # data = random.random() if ndis > 380 and ndis < 420: datad += 0.35 print('datad::', datad) print(fYaw) # data = 0.5 data2 = X sock2.sendto(struct.pack('<dd', datac, datad), ip_port) #sock2.sendto(struct.pack('<d', datad), addr2) #time.sleep(0.01) # sock.sendto(data,("192.168.1.107", 9005)) cv2.imshow('tp', nimg) ''' print 'L' print ((xa[0], ya[0]), (xb[0], yb[0]),kl) print 'R' print ((x1[0], y1[0]), (x2[0], y2[0]),k1) ''' if (kl < -slopeL): # if float(xb) < leftXR and float(xb) > leftXL: cv2.line(frame, (predicted[0][0], predicted[0][1]), (predicted[0][2], predicted[0][3]), (0, 0, 255), 5) if (k1 > slopeR): # if float(x1) < leftXR and float(x1) > leftXL: cv2.line(frame, (predicted[1][0], predicted[1][1]), (predicted[1][2], predicted[1][3]), (0, 0, 255), 5) lt.update(lanes) # print(frame.shape) cv2.imshow('demo', frame) if cv2.waitKey(1) & 0xFF == ord('s'): s = 1 - s if cv2.waitKey(1) & 0xFF == ord('q'): # 1 break
import numpy as np import cv2 from moviepy.editor import VideoFileClip import argparse import json import track import detect # Import arguments parser = argparse.ArgumentParser() parser.add_argument('--videopath', type=str, required=True) args = parser.parse_args() lt = track.LaneTracker(2, 0.1, 500) ld = detect.LaneDetector(180) lines1 = {} lines2 = {} it = 0 ticks = 0 iti = 0 def track(frame): global lt, ld, ticks, it precTick = ticks ticks = cv2.getTickCount()