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.

223 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. product = sum(imgs[0][:,SIZE-overlaps:].astype(np.uint32)*imgs[1][:,:overlaps].astype(np.uint32))
  104. product += sum(imgs[2][:,SIZE-overlaps:].astype(np.uint32)*imgs[3][:,:overlaps].astype(np.uint32))
  105. product = sum(product) / len(product)
  106. maxProduct = product
  107. tmp = maxProduct
  108. maxProduct = 0
  109. product = sum(imgs[0][SIZE-overlaps:, :].astype(np.uint32)*imgs[2][:overlaps,:].astype(np.uint32))
  110. product += sum(imgs[1][SIZE-overlaps:, :].astype(np.uint32)*imgs[3][:overlaps,:].astype(np.uint32))
  111. product = sum(product) / len(product)
  112. maxProduct = product
  113. maxProduct = (tmp + maxProduct)/2
  114. if maxProduct > PRODUCTION_THRESHOLD:
  115. tmp = np.zeros((SIZE, SIZE*2-overlap_w), dtype=np.uint16)
  116. tmp[:, :SIZE] = imgs[0]
  117. tmp[:, -SIZE:] += imgs[1]
  118. tmp[:, (SIZE-overlap_w): SIZE] = tmp[:, (SIZE-overlap_w): SIZE]/2
  119. tmp2 = np.zeros((SIZE, SIZE*2-overlap_w), dtype=np.uint16)
  120. tmp2[:, :SIZE] = imgs[2]
  121. tmp2[:, -SIZE:] += imgs[3]
  122. tmp2[:, (SIZE-overlap_w): SIZE] = tmp2[:, (SIZE-overlap_w): SIZE]/2
  123. merge = np.zeros((SIZE*2-overlap_h, SIZE*2-overlap_w), dtype=np.uint16)
  124. merge[:SIZE, :] = tmp
  125. merge[-SIZE:, :] += tmp2
  126. merge[(SIZE-overlap_h):SIZE, :] = merge[(SIZE-overlap_h):SIZE, :]/2
  127. position = [0,0]
  128. rows,cols = merge.shape
  129. for i in range(rows):
  130. for j in range(cols):
  131. position[0] += i*merge[i][j]
  132. position[1] += j*merge[i][j]
  133. position[0] /= sum(sum(merge))
  134. position[1] /= sum(sum(merge))
  135. offset_w = int(overlap_w/2)
  136. offset_h = int(overlap_h/2)
  137. out[SIZE*2+offset_h:SIZE*4-overlap_h+offset_h, offset_w: SIZE*2-overlap_w+offset_w] = merge
  138. pos_w = 1.17*position[0] #distanceBetweenSensors_w/(SIZE-overlap_w)*position[0]
  139. pos_h = 1.17*position[1] #distanceBetweenSensors_h/(SIZE-overlap_h)*position[1]
  140. if not hasPos:
  141. startPos = [pos_w, pos_h]
  142. sp = position
  143. path = []
  144. truePath = []
  145. times = []
  146. startTime = frames[0].time
  147. hasPos = True
  148. if not innerHasPos and pos_w >= 16 and pos_w <= 109 and pos_h >= 16 and pos_h <= 109:
  149. innerStartPos = [pos_w, pos_h]
  150. innerStartTime = frames[0].time
  151. innerHasPos = True
  152. if pos_w >= 16 and pos_w <= 109 and pos_h >= 16 and pos_h <= 109:
  153. innerEndPos = [pos_w, pos_h]
  154. innerEndTime = frames[0].time
  155. elif innerHasPos:
  156. if innerEndTime - innerStartTime > 0:
  157. print (innerStartPos, innerEndPos)
  158. print ('inner speed:', ((innerEndPos[0]-innerStartPos[0])**2+(innerEndPos[1]-innerStartPos[1])**2)**0.5/(innerEndTime - innerStartTime))
  159. print ('time:', innerEndTime-innerStartTime)
  160. innerHasPos = False
  161. endPos = [pos_w, pos_h]
  162. endTime = frames[0].time
  163. path.append(position)
  164. truePath.append(endPos)
  165. times.append(frames[0].time)
  166. elif hasPos:
  167. if endTime - startTime > 0:
  168. print (startPos, endPos)
  169. print ('speed:', ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime))
  170. print ('time:', endTime-startTime)
  171. if innerHasPos and innerEndTime - innerStartTime > 0:
  172. print (innerStartPos, innerEndPos)
  173. print ('inner speed:', ((innerEndPos[0]-innerStartPos[0])**2+(innerEndPos[1]-innerStartPos[1])**2)**0.5/(innerEndTime - innerStartTime))
  174. print ('time:', innerEndTime-innerStartTime)
  175. hasPos = False
  176. innerHasPos = False
  177. out = out.astype(np.uint8)
  178. out = exponential(out, EXPONENTAL_VALUE)
  179. out = cv2.cvtColor(out,cv2.COLOR_GRAY2BGR)
  180. if endTime - startTime > MIN_EXIST_TIME:
  181. speed = ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime)
  182. cv2.putText(out, f'{speed:.2f}',
  183. (0, SIZE*2),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
  184. speed = ((truePath[-1][0]-truePath[-2][0])**2+(truePath[-1][1]-truePath[-2][1])**2)**0.5/(times[-1] - times[-2])
  185. cv2.putText(out, f'{speed:.2f}',
  186. (0, SIZE*2+30),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
  187. if maxProduct > PRODUCTION_THRESHOLD:
  188. cv2.circle(out, (offset_w+int(position[1]), SIZE*2+offset_h+int(position[0])), 10, (255,0,0), 5)
  189. cv2.circle(out, (offset_w+int(sp[1]), SIZE*2+offset_h+int(sp[0])), 10, (0,255,0), 5)
  190. for i in range(len(path)-1):
  191. 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))
  192. cv2.line
  193. lastFrame = out[SIZE*2:,:]
  194. hasLastFrame = True
  195. elif hasLastFrame:
  196. out[SIZE*2:,:] = lastFrame
  197. cv2.imshow('sample', out)
  198. videoWriter.write(out)
  199. key = cv2.waitKey(1)
  200. if key == ord('q'):
  201. break
  202. videoWriter.release()
  203. cv2.destroyAllWindows()