API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 19th, 2018
73
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 12.96 KB | None | 0 0
raw download clone embed print report
  1. from picamera.array import PiRGBArray as pcarray
  2. from picamera import PiCamera as picam
  3. import cv2
  4. import time
  5. from datetime import datetime
  6. from PiLo import PiLo
  7. import numpy as np
  8. import sys
  9. import atexit
  10.  
  11. class trim:
  12. def __init__(self):
  13. self.l = 0
  14. self.r = 0
  15. self.ml = 6
  16. self.mr = 6
  17. self.base = 35
  18.  
  19.  
  20. def exit_handler():
  21. try:
  22. out.release()
  23. cap.release()
  24. cv2.destroyAllWindows()
  25. except Exception:
  26. print("Could not release capture device, if you are on a Pi this is normal")
  27. finally:
  28. d = (datetime.now() - start).microseconds
  29. print(str(d))
  30. fps = framecount / (d / 10000.00)
  31. p.sendCommand(1, 0, 0)
  32. print(str(fps))
  33. atexit.register(exit_handler)
  34. def filter_region(image, vertices):
  35. """
  36. Create the mask using the vertices and apply it to the input image
  37. """
  38. mask = np.zeros_like(image)
  39. if len(mask.shape)==2:
  40. cv2.fillPoly(mask, vertices, 255)
  41.  
  42. else:
  43. cv2.fillPoly(mask, vertices, (255,)*mask.shape[2]) # in case, the input image has a channel dimension
  44. return cv2.bitwise_and(image, mask)
  45.  
  46. def average_slope_intercept(lines):
  47. left_lines = [] # (slope, intercept)
  48. left_weights = [] # (length,)
  49. right_lines = [] # (slope, intercept)
  50. right_weights = [] # (length,)
  51.  
  52. for line in lines:
  53. for x1, y1, x2, y2 in line:
  54. if x2==x1:
  55. continue # ignore a vertical line
  56. slope = (y2-y1)/(x2-x1)
  57. intercept = y1 - slope*x1
  58. length = np.sqrt((y2-y1)**2+(x2-x1)**2)
  59. if slope < 0: # y is reversed in image
  60. left_lines.append((slope, intercept))
  61. left_weights.append((length))
  62. else:
  63. right_lines.append((slope, intercept))
  64. right_weights.append((length))
  65.  
  66. # add more weight to longer lines
  67. left_lane = np.dot(left_weights, left_lines) /np.sum(left_weights) if len(left_weights) >0 else None
  68. right_lane = np.dot(right_weights, right_lines)/np.sum(right_weights) if len(right_weights)>0 else None
  69.  
  70. return left_lane, right_lane # (slope, intercept), (slope, intercept)
  71.  
  72. def select_region(image):
  73. """
  74. It keeps the region surrounded by the `vertices` (i.e. polygon). Other area is set to 0 (black).
  75. """
  76. # first, define the polygon by vertices
  77. rows, cols = image.shape[:2]
  78. #print(str(rows) + "\n" + str(cols))
  79. bottom_left = [0, rows]
  80. top_left = [cols*0.35, rows*0.35]
  81. bottom_right = [cols , rows]
  82. top_right = [cols*0.65, rows*0.35]
  83. # the vertices are an array of polygons (i.e array of arrays) and the data type must be integer
  84. vertices = np.array([[bottom_left, top_left, top_right, bottom_right]], dtype=np.int32)
  85.  
  86. return filter_region(image, vertices)
  87.  
  88. def make_line_points(y1, y2, line):
  89. """
  90. Convert a line represented in slope and intercept into pixel points
  91. """
  92. if line is None:
  93. return None
  94.  
  95. slope, intercept = line
  96. print("Slope: " + str(slope))
  97. # make sure everything is integer as cv2.line requires it
  98. try:
  99. x1 = int((y1 - intercept)/slope)
  100. x2 = int((y2 - intercept)/slope)
  101.  
  102. px = int(((y2 ) - intercept)/slope)
  103. except OverflowError:
  104. return (None, None), None
  105.  
  106. y1 = int(y1)
  107. y2 = int(y2)
  108.  
  109. return ((x1, y1), (x2, y2)), px
  110.  
  111. def lane_lines(image, lines):
  112. left_lane, right_lane = average_slope_intercept(lines)
  113.  
  114. y1 = image.shape[0] # bottom of the image
  115. y2 = y1*0.7 # slightly lower than the middle
  116. try:
  117. left_line, lp = make_line_points(y1, y2, left_lane)
  118. except TypeError:
  119. left_line = None
  120. lp = None
  121. try:
  122. right_line, rp = make_line_points(y1, y2, right_lane)
  123. except TypeError:
  124. right_line = None
  125. rp = None
  126. print(str(left_line))
  127. print(str(right_line))
  128. print("LP: " + str(lp))
  129. print("RP: " + str(rp))
  130. return (left_line, right_line), lp, rp
  131.  
  132.  
  133. def draw_lane_lines(image, lines, color=[255, 0, 0], thickness=20):
  134. # make a separate image to draw lines and combine with the orignal later
  135. line_image = np.zeros_like(image)
  136. for line in lines:
  137. if line is not None:
  138. print(str(line))
  139. cv2.line(line_image, *line, color, thickness)
  140.  
  141. return cv2.addWeighted(image, 1.0, line_image, 0.95, 0.0)
  142.  
  143. global t
  144. t = trim()
  145. def process(frame, lanefail):
  146. text = "Nothing to report"
  147. textr = "Nothing to report"
  148. code = 0
  149. lc = False
  150. rc = False
  151. froi = select_region(frame)
  152. hsv2 = cv2.cvtColor(frame, cv2.COLOR_BGR2HLS)
  153. lower_red = np.uint8([ 10, 0, 100])
  154. upper_red = np.uint8([255, 255, 255])
  155.  
  156. # Here we are defining range of bluecolor in HSV
  157. # This creates a mask of blue coloured
  158. # objects found in the frame.
  159. hsv = cv2.inRange(hsv2, lower_red, upper_red)
  160. # frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
  161. img = cv2.GaussianBlur(hsv, (5,5), 0)
  162.  
  163. canny = cv2.Canny(hsv, 100, 255)
  164. if "--noroi" in sys.argv:
  165. roi = canny
  166. else:
  167. roi = select_region(canny)
  168. h, w, c = 0,0,0
  169. lines = cv2.HoughLinesP(roi, rho=1, theta=np.pi/180, threshold=30, minLineLength=20, maxLineGap=200)
  170.  
  171. if lines is not None:
  172. lanefail -= 2
  173. if lanefail < 0:
  174. lanefail = 0
  175. lines, lp, rp = lane_lines(frame, lines)
  176. h, w, c = frame.shape
  177. #print("Height:" + str(h) )q
  178. adj = (h - 200)
  179.  
  180.  
  181. cv2.line(frame,(int(w* 0.5), 0), (int(w * 0.5),h),(0,0,255),2)
  182. cv2.line(frame,(int(w * 0.3), 0), (int(w * 0.3),h), (255,0,255),2)
  183. cv2.line(frame,(int(w * 0.7), 0), (int(w * 0.7),h), (0,150,255),2)
  184.  
  185.  
  186. if lp is not None:
  187.  
  188. if lp >= w * 0.3 and lp < w * 0.5 :
  189. cv2.line(frame,(int(w * 0.3), 0), (int(w * 0.3),h), (150,0,255),2)
  190. text = "Left lane prox, soft correct"
  191. code = -1
  192. elif lp >= w * 0.3 and lp < w * 0.6:
  193. cv2.line(frame,(int(w* 0.4), 0), (int(w * 0.4),h),(0,0,255),3)
  194. text = "Left lane collision, hard correct!"
  195. code = -2
  196. else:
  197. text = "Left Clear"
  198. lc = True
  199.  
  200. lpr = (lp, adj)
  201. cv2.circle(frame, lpr, 5, thickness=5, color=[0,0,255])
  202.  
  203. if rp is not None:
  204. if rp <= w * 0.7 and rp > w * 0.3:
  205.  
  206. textr = "Right lane prox, soft correct"
  207. code = 1
  208. cv2.line(frame,(int(w * 0.7), 0), (int(w * 0.7),h), (0,150,255),2)
  209. elif rp <= w * 0.7 and rp > w * 0.4:
  210. cv2.line(frame,(int(w* 0.6), 0), (int(w * 0.6),h),(0,0,255),3)
  211. textr = "Right lane collision, hard correct!"
  212. code = 2
  213. else:
  214.  
  215. textr = "Right Clear"
  216. rc = True
  217. rpr = (rp, adj)
  218. cv2.circle(frame, rpr, 5, thickness=5, color=[0,255,0])
  219. # if lc and rc:
  220. # code = "s"
  221. # elif lc:
  222. # code = 0
  223. #
  224. # elif rc:
  225. # code = 0
  226.  
  227. if lp is not None and rp is not None:
  228.  
  229. if abs(lp - rp) < w * 0.10:
  230. code = 0
  231. text = "Center Line Possible, Drive Straight"
  232. textr = "Center Line Possible, Drive Straight"
  233.  
  234. frame = draw_lane_lines(frame, lines)
  235.  
  236. else:
  237. lanefail += 1
  238.  
  239. if lanefail > 20:
  240. if lanefail > 50:
  241. lanefail = 50
  242. text = "Lane detection is dirty, check threshold"
  243. textr = "Lane detection is dirty, check threshold"
  244. t.base -= 1
  245. if t.base < 0:
  246. t.base = 0
  247. else:
  248. t.base = 35
  249. #cv2.rectangle(frame,(0,0),(int(w*0.5) , int(h* 0.35)),(0,0,0) ,-1)
  250.  
  251. cv2.putText(frame,text,(10, 50), cv2.FONT_HERSHEY_SIMPLEX,0.35,(255,200,10))
  252. cv2.putText(frame,textr,(w-50, 50),cv2.FONT_HERSHEY_SIMPLEX,0.35,(255,200,10))
  253. print("LT: " + text)
  254. print("RT: " + textr)
  255.  
  256. return frame, roi, canny, lanefail, text, textr, code
  257.  
  258.  
  259. p = PiLo()
  260.  
  261. start = datetime.now()
  262. if "-v" in sys.argv:
  263. fourcc = cv2.VideoWriter_fourcc(*'XVID')
  264. out = cv2.VideoWriter('output.avi', fourcc, 20.0, (360,240), True)
  265. # images showing the region of interest only
  266. framecount = 0
  267.  
  268. if "--raspi" in sys.argv:
  269. camera = picam()
  270. camera.resolution = (480,240)
  271. camera.framerate = (30)
  272. camera.hflip = True
  273. if "--flip" in sys.argv:
  274. camera.vflip = True
  275.  
  276. rc = pcarray(camera)
  277. time.sleep(0.1)
  278. print("Using On-Board PiCamera")
  279. usepi = True
  280. path = "blank"
  281. else:
  282. usepi = False
  283. path = sys.argv[1].strip()
  284. if path == "0":
  285. path = 0
  286. cap = cv2.VideoCapture(path)
  287. cap.set(3,360)
  288. cap.set(4,240)
  289.  
  290.  
  291. lanefail = 0
  292.  
  293. if ".avi" in str(path):
  294. wait = 166
  295. else:
  296. wait = 1
  297.  
  298. if "--raspi" in sys.argv:
  299. for cap in camera.capture_continuous(rc, format="bgr", use_video_port=True):
  300. frame = cap.array
  301.  
  302. frame, roi, canny, lanefail, text, textr, code = process(frame, lanefail)
  303. print(str(code))
  304. adj = 1
  305. if code != 0:
  306. t.l += -code * adj
  307. t.r += code * adj
  308. # elif code == 0:
  309. # print("Detection failed, continuing previous")
  310.  
  311. else:
  312. if t.l > 0:
  313. t.l -= adj
  314. elif t.l < 0:
  315. t.l += adj
  316. if t.r > 0:
  317. t.r -= adj
  318. elif t.r < 0:
  319. t.r += adj
  320.  
  321. if t.l > t.ml:
  322. t.l = t.ml
  323. if t.l <= -t.ml:
  324. t.l = -t.ml
  325.  
  326. if t.r > t.mr:
  327. t.r = t.mr
  328. if t.r <= -t.mr:
  329. t.r = -t.mr
  330.  
  331.  
  332. print(str(t.l))
  333. print(str(t.r))
  334. print(str(t.base))
  335.  
  336. p.sendCommand(1, t.base + t.l, t.base + t.r)
  337.  
  338. if "--cont" not in sys.argv:
  339. if framecount > 240:
  340. break
  341. framecount += 1
  342. if "-v" in sys.argv:
  343. print(str(frame.shape))
  344. frame = cv2.resize(frame,(360,240))
  345. print(str(frame.shape))
  346. out.write(frame)
  347. rc.truncate(0)
  348. if "--nodisp" not in sys.argv:
  349. #cv2.imshow('Canny', canny)
  350. cv2.imshow('Original', frame)
  351. cv2.imshow('ROI', roi)
  352. #cv2.imshow('FROI', froi)
  353.  
  354. if cv2.waitKey(wait) & 0xFF == ord('q'):
  355. break
  356. print("FRAME: " + str(framecount))
  357. else:
  358. try:
  359. while 1:
  360. ret, frame = cap.read()
  361. frame, roi, canny, lanefail, text, textr, code= process(frame, lanefail)
  362.  
  363. adj = 2
  364. if code != 0:
  365. t.l += -code * adj
  366. t.r += code * adj
  367. # elif code == 0:
  368. # print("Detection failed, continuing previous")
  369.  
  370. else:
  371. if t.l > 0:
  372. t.l -= adj
  373. elif t.l < 0:
  374. t.l += adj
  375. if t.r > 0:
  376. t.r -= adj
  377. elif t.r < 0:
  378. t.r += adj
  379.  
  380. if t.l > t.ml:
  381. t.l = t.ml
  382. if t.l <= -t.ml:
  383. t.l = -t.ml
  384.  
  385. if t.r > t.mr:
  386. t.r = t.mr
  387. if t.r <= -t.mr:
  388. t.r = -t.mr
  389.  
  390.  
  391. print(str(t.l))
  392. print(str(t.r))
  393. print(str(t.base))
  394.  
  395. p.sendCommand(1, t.base + t.l, t.base + t.r)
  396.  
  397. if "--cont" not in sys.argv:
  398. if framecount > 120:
  399. break
  400. framecount += 1
  401. if "-v" in sys.argv and ret:
  402. print(str(frame.shape))
  403. frame = cv2.resize(frame,(320,240))
  404. try:
  405. # print(str(frame.shape))
  406. out.write(frame)
  407. print("Written")
  408. except:
  409. print("failed")
  410. if "--nodisp" not in sys.argv:
  411. #cv2.imshow('Canny', canny)
  412. cv2.imshow('Original', frame)
  413. cv2.imshow('ROI', roi)
  414. #cv2.imshow('FROI', froi)
  415.  
  416. if cv2.waitKey(wait) & 0xFF == ord('q'):
  417. break
  418.  
  419. framecount += 1
  420. print("FRAME: " + str(framecount))
  421. except Exception as e:
  422. print("Exception: " + str(e))
  423. exit_handler()
  424. try:
  425. out.release()
  426. print("Out released")
  427. cap.release()
  428. cv2.destroyAllWindows()
  429. except Exception:
  430. print("Could not release capture device, if you are on a Pi this is normal")
  431. finally:
  432. d = (datetime.now() - start).microseconds
  433. print(str(d))
  434. fps = framecount / (d / 10000.00)
  435. p.sendCommand(1, 0, 0)
  436. print(str(fps))
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!