555
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

229 lines
9.3 KiB

  1. import json
  2. class Frame():
  3. def __init__(self, time, data):
  4. self.time = time
  5. self.data = data
  6. class GridEyeData():
  7. def __init__(self, filePath):
  8. self.f = open(filePath, 'r')
  9. self.frames = [None]*4
  10. def readFrame(self):
  11. time = self.f.readline()
  12. if not time:
  13. return False
  14. time = float(time)
  15. for i in range(4):
  16. data = json.loads(self.f.readline())
  17. self.frames[i] = Frame(time, data)
  18. return True
  19. if __name__ == '__main__':
  20. import cv2
  21. import numpy as np
  22. import sys
  23. def exponential(img, value):
  24. tmp = cv2.pow(img.astype(np.double), value)*(255.0/(255.0**value))
  25. return tmp.astype(np.uint8)
  26. SIZE = 128
  27. AVERAGE_FRAME = 10
  28. distanceBetweenSensors_w = 2.6 #cm
  29. distanceBetweenSensors_h = 2.6 #cm
  30. distance2Object = 60.0 #cm
  31. ADJUST_BACK = 5
  32. EXPONENTAL_VALUE = 0.4
  33. PRODUCTION_THRESHOLD = 10
  34. MIN_EXIST_TIME = 0.1
  35. cnt = 0
  36. avers = []
  37. fourcc = cv2.VideoWriter_fourcc(*'XVID')
  38. videoWriter = cv2.VideoWriter('output.avi', fourcc, 10.0, (SIZE*2,SIZE*4))
  39. cv2.imshow('sample', np.zeros((SIZE*4,SIZE*2), np.uint8))
  40. gridEye = GridEyeData(sys.argv[1])
  41. hasLastFrame = False
  42. hasPos = False
  43. innerHasPos = False
  44. endTime = 0
  45. startTime = 0
  46. innerEndTime = 0
  47. innerStartTime = 0
  48. path = []
  49. speed = 0
  50. while gridEye.readFrame():
  51. frames = gridEye.frames
  52. imgs = []
  53. for frame in frames:
  54. img = (np.array(frame.data)-15)*10
  55. img = cv2.resize(img.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_LINEAR) # INTER_LINEAR, INTER_CUBIC
  56. imgs.append(img)
  57. avers.append(np.zeros((SIZE,SIZE), np.uint16))
  58. if cnt < AVERAGE_FRAME:
  59. cnt += 1
  60. for i in range(len(imgs)):
  61. avers[i] += imgs[i]
  62. if cnt == AVERAGE_FRAME:
  63. for i in range(len(avers)):
  64. avers[i] = avers[i]/AVERAGE_FRAME
  65. avers[i] = avers[i].astype(np.uint8)
  66. avers[i] += ADJUST_BACK
  67. continue
  68. for i in range(len(imgs)):
  69. imgs[i] = cv2.subtract(imgs[i], avers[i])
  70. out = np.full((SIZE*4, SIZE*2), 255, dtype=np.uint16)
  71. out[:SIZE, :SIZE] = imgs[0]
  72. out[:SIZE, SIZE:SIZE*2] = imgs[1]
  73. out[SIZE:SIZE*2, :SIZE] = imgs[2]
  74. out[SIZE:SIZE*2, SIZE:SIZE*2] = imgs[3]
  75. # production
  76. '''
  77. maxProduct = 0
  78. overlap_w = 0
  79. for i in range(80, 128):
  80. product = sum(imgs[0][:,SIZE-i:].astype(np.uint32)*imgs[1][:,:i].astype(np.uint32))
  81. product += sum(imgs[2][:,SIZE-i:].astype(np.uint32)*imgs[3][:,:i].astype(np.uint32))
  82. product = sum(product) / len(product)
  83. if product > maxProduct:
  84. maxProduct = product
  85. overlap_w = i
  86. tmp = maxProduct
  87. maxProduct = 0
  88. overlap_h = 0
  89. for i in range(80, 128):
  90. product = sum(imgs[0][SIZE-i:, :].astype(np.uint32)*imgs[2][:i,:].astype(np.uint32))
  91. product += sum(imgs[1][SIZE-i:, :].astype(np.uint32)*imgs[3][:i,:].astype(np.uint32))
  92. product = sum(product) / len(product)
  93. if product > maxProduct:
  94. maxProduct = product
  95. overlap_h = i
  96. maxProduct = (tmp + maxProduct)/2
  97. # fixed overlap_h
  98. '''
  99. maxProduct = 0
  100. overlaps = 125
  101. overlap_w = overlaps
  102. overlap_h = overlaps
  103. '''
  104. product = sum(imgs[0][:,SIZE-overlaps:].astype(np.uint32)*imgs[1][:,:overlaps].astype(np.uint32))
  105. product += sum(imgs[2][:,SIZE-overlaps:].astype(np.uint32)*imgs[3][:,:overlaps].astype(np.uint32))
  106. product = sum(product) / len(product)
  107. maxProduct = product
  108. tmp = maxProduct
  109. maxProduct = 0
  110. product = sum(imgs[0][SIZE-overlaps:, :].astype(np.uint32)*imgs[2][:overlaps,:].astype(np.uint32))
  111. product += sum(imgs[1][SIZE-overlaps:, :].astype(np.uint32)*imgs[3][:overlaps,:].astype(np.uint32))
  112. product = sum(product) / len(product)
  113. maxProduct = product
  114. maxProduct = (tmp + maxProduct)/2
  115. '''
  116. #if maxProduct > PRODUCTION_THRESHOLD:
  117. if True:
  118. tmp = np.zeros((SIZE, SIZE*2-overlap_w), dtype=np.uint16)
  119. tmp[:, :SIZE] = imgs[0]
  120. tmp[:, -SIZE:] += imgs[1]
  121. tmp[:, (SIZE-overlap_w): SIZE] = tmp[:, (SIZE-overlap_w): SIZE]/2
  122. tmp2 = np.zeros((SIZE, SIZE*2-overlap_w), dtype=np.uint16)
  123. tmp2[:, :SIZE] = imgs[2]
  124. tmp2[:, -SIZE:] += imgs[3]
  125. tmp2[:, (SIZE-overlap_w): SIZE] = tmp2[:, (SIZE-overlap_w): SIZE]/2
  126. merge = np.zeros((SIZE*2-overlap_h, SIZE*2-overlap_w), dtype=np.uint16)
  127. merge[:SIZE, :] = tmp
  128. merge[-SIZE:, :] += tmp2
  129. merge[(SIZE-overlap_h):SIZE, :] = merge[(SIZE-overlap_h):SIZE, :]/2
  130. offset_w = int(overlap_w/2)
  131. offset_h = int(overlap_h/2)
  132. out[SIZE*2+offset_h:SIZE*4-overlap_h+offset_h, offset_w: SIZE*2-overlap_w+offset_w] = merge
  133. '''
  134. position = [0,0]
  135. rows,cols = merge.shape
  136. for i in range(rows):
  137. for j in range(cols):
  138. position[0] += i*merge[i][j]
  139. position[1] += j*merge[i][j]
  140. position[0] /= sum(sum(merge))
  141. position[1] /= sum(sum(merge))
  142. pos_w = 1.17*position[0] #distanceBetweenSensors_w/(SIZE-overlap_w)*position[0]
  143. pos_h = 1.17*position[1] #distanceBetweenSensors_h/(SIZE-overlap_h)*position[1]
  144. if not hasPos:
  145. startPos = [pos_w, pos_h]
  146. sp = position
  147. path = []
  148. truePath = []
  149. times = []
  150. startTime = frames[0].time
  151. hasPos = True
  152. if not innerHasPos and pos_w >= 16 and pos_w <= 109 and pos_h >= 16 and pos_h <= 109:
  153. innerStartPos = [pos_w, pos_h]
  154. innerStartTime = frames[0].time
  155. innerHasPos = True
  156. if pos_w >= 16 and pos_w <= 109 and pos_h >= 16 and pos_h <= 109:
  157. innerEndPos = [pos_w, pos_h]
  158. innerEndTime = frames[0].time
  159. elif innerHasPos:
  160. if innerEndTime - innerStartTime > 0:
  161. print (innerStartPos, innerEndPos)
  162. print ('inner speed:', ((innerEndPos[0]-innerStartPos[0])**2+(innerEndPos[1]-innerStartPos[1])**2)**0.5/(innerEndTime - innerStartTime))
  163. print ('time:', innerEndTime-innerStartTime)
  164. innerHasPos = False
  165. endPos = [pos_w, pos_h]
  166. endTime = frames[0].time
  167. path.append(position)
  168. truePath.append(endPos)
  169. times.append(frames[0].time)
  170. '''
  171. elif hasPos:
  172. if endTime - startTime > 0:
  173. print (startPos, endPos)
  174. print ('speed:', ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime))
  175. print ('time:', endTime-startTime)
  176. if innerHasPos and innerEndTime - innerStartTime > 0:
  177. print (innerStartPos, innerEndPos)
  178. print ('inner speed:', ((innerEndPos[0]-innerStartPos[0])**2+(innerEndPos[1]-innerStartPos[1])**2)**0.5/(innerEndTime - innerStartTime))
  179. print ('time:', innerEndTime-innerStartTime)
  180. hasPos = False
  181. innerHasPos = False
  182. out = out.astype(np.uint8)
  183. out = exponential(out, EXPONENTAL_VALUE)
  184. out = cv2.cvtColor(out,cv2.COLOR_GRAY2BGR)
  185. '''
  186. if endTime - startTime > MIN_EXIST_TIME:
  187. speed = ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime)
  188. cv2.putText(out, f'{speed:.2f}',
  189. (0, SIZE*2),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
  190. speed = ((truePath[-1][0]-truePath[-2][0])**2+(truePath[-1][1]-truePath[-2][1])**2)**0.5/(times[-1] - times[-2])
  191. cv2.putText(out, f'{speed:.2f}',
  192. (0, SIZE*2+30),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
  193. if maxProduct > PRODUCTION_THRESHOLD:
  194. cv2.circle(out, (offset_w+int(position[1]), SIZE*2+offset_h+int(position[0])), 10, (255,0,0), 5)
  195. cv2.circle(out, (offset_w+int(sp[1]), SIZE*2+offset_h+int(sp[0])), 10, (0,255,0), 5)
  196. for i in range(len(path)-1):
  197. cv2.line(out, (offset_w+int(path[i][1]), SIZE*2+offset_h+int(path[i][0])), (offset_w+int(path[i+1][1]), SIZE*2+offset_h+int(path[i+1][0])), (0,0,255))
  198. cv2.line
  199. lastFrame = out[SIZE*2:,:]
  200. hasLastFrame = True
  201. elif hasLastFrame:
  202. out[SIZE*2:,:] = lastFrame
  203. '''
  204. cv2.imshow('sample', out)
  205. videoWriter.write(out)
  206. key = cv2.waitKey(1)
  207. if key == ord('q'):
  208. break
  209. videoWriter.release()
  210. cv2.destroyAllWindows()