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.
247 lines
10 KiB
247 lines
10 KiB
import json
|
|
|
|
class Frame():
|
|
def __init__(self, time, data):
|
|
self.time = time
|
|
self.data = data
|
|
|
|
class GridEyeData():
|
|
def __init__(self, filePath):
|
|
self.f = open(filePath, 'r')
|
|
self.frames = [None]*4
|
|
def readFrame(self):
|
|
time = self.f.readline()
|
|
if not time:
|
|
return False
|
|
time = float(time)
|
|
for i in range(4):
|
|
data = json.loads(self.f.readline())
|
|
self.frames[i] = Frame(time, data)
|
|
return True
|
|
|
|
if __name__ == '__main__':
|
|
import cv2
|
|
import numpy as np
|
|
import sys
|
|
from functools import reduce
|
|
def exponential(img, value):
|
|
tmp = cv2.pow(img.astype(np.double), value)*(255.0/(255.0**value))
|
|
return tmp.astype(np.uint8)
|
|
|
|
SIZE = 128
|
|
AVERAGE_FRAME = 10
|
|
distanceBetweenSensors_w = 2.6 #cm
|
|
distanceBetweenSensors_h = 2.6 #cm
|
|
distance2Object = 60.0 #cm
|
|
ADJUST_BACK = 5
|
|
EXPONENTAL_VALUE = 0.4
|
|
PRODUCTION_THRESHOLD = 10
|
|
MIN_EXIST_TIME = 0.1
|
|
cnt = 0
|
|
avers = []
|
|
|
|
raw_aver = np.array([0]*64*4, np.float64)
|
|
raw_aver2 = np.array([0]*64*4, np.float64)
|
|
|
|
fourcc = cv2.VideoWriter_fourcc(*'XVID')
|
|
videoWriter = cv2.VideoWriter('output.avi', fourcc, 10.0, (SIZE*2,SIZE*4))
|
|
cv2.imshow('sample', np.zeros((SIZE*4,SIZE*2), np.uint8))
|
|
gridEye = GridEyeData(sys.argv[1])
|
|
|
|
hasLastFrame = False
|
|
hasPos = False
|
|
innerHasPos = False
|
|
endTime = 0
|
|
startTime = 0
|
|
innerEndTime = 0
|
|
innerStartTime = 0
|
|
path = []
|
|
speed = 0
|
|
avers.append(np.zeros((SIZE,SIZE), np.uint16))
|
|
avers.append(np.zeros((SIZE,SIZE), np.uint16))
|
|
avers.append(np.zeros((SIZE,SIZE), np.uint16))
|
|
avers.append(np.zeros((SIZE,SIZE), np.uint16))
|
|
while gridEye.readFrame():
|
|
frames = gridEye.frames
|
|
imgs = []
|
|
raw = []
|
|
for frame in frames:
|
|
img = (np.array(frame.data)-15)*10
|
|
img = cv2.resize(img.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_LINEAR) # INTER_LINEAR, INTER_CUBIC
|
|
imgs.append(img)
|
|
raw += reduce(lambda x,y: x+y, frame.data)
|
|
raw_aver += np.array(raw)
|
|
raw_aver2 += np.array(raw)**2
|
|
if cnt < AVERAGE_FRAME:
|
|
cnt += 1
|
|
for i in range(len(imgs)):
|
|
avers[i] += imgs[i]
|
|
if cnt == AVERAGE_FRAME:
|
|
b = (raw_aver/AVERAGE_FRAME)**2
|
|
a = raw_aver2/AVERAGE_FRAME
|
|
print ('aver', raw_aver/AVERAGE_FRAME)
|
|
print ((a-b)**0.5)
|
|
print (sum((a-b)**0.5)/64/4)
|
|
exit()
|
|
for i in range(len(avers)):
|
|
avers[i] = avers[i]/AVERAGE_FRAME
|
|
avers[i] = avers[i].astype(np.uint8)
|
|
avers[i] += ADJUST_BACK
|
|
continue
|
|
|
|
for i in range(len(imgs)):
|
|
imgs[i] = cv2.subtract(imgs[i], avers[i])
|
|
out = np.full((SIZE*4, SIZE*2), 255, dtype=np.uint16)
|
|
out[:SIZE, :SIZE] = imgs[0]
|
|
out[:SIZE, SIZE:SIZE*2] = imgs[1]
|
|
out[SIZE:SIZE*2, :SIZE] = imgs[2]
|
|
out[SIZE:SIZE*2, SIZE:SIZE*2] = imgs[3]
|
|
|
|
# production
|
|
'''
|
|
maxProduct = 0
|
|
overlap_w = 0
|
|
for i in range(80, 128):
|
|
product = sum(imgs[0][:,SIZE-i:].astype(np.uint32)*imgs[1][:,:i].astype(np.uint32))
|
|
product += sum(imgs[2][:,SIZE-i:].astype(np.uint32)*imgs[3][:,:i].astype(np.uint32))
|
|
product = sum(product) / len(product)
|
|
if product > maxProduct:
|
|
maxProduct = product
|
|
overlap_w = i
|
|
tmp = maxProduct
|
|
maxProduct = 0
|
|
overlap_h = 0
|
|
for i in range(80, 128):
|
|
product = sum(imgs[0][SIZE-i:, :].astype(np.uint32)*imgs[2][:i,:].astype(np.uint32))
|
|
product += sum(imgs[1][SIZE-i:, :].astype(np.uint32)*imgs[3][:i,:].astype(np.uint32))
|
|
product = sum(product) / len(product)
|
|
if product > maxProduct:
|
|
maxProduct = product
|
|
overlap_h = i
|
|
maxProduct = (tmp + maxProduct)/2
|
|
|
|
# fixed overlap_h
|
|
'''
|
|
|
|
maxProduct = 0
|
|
overlaps = 125
|
|
overlap_w = overlaps
|
|
overlap_h = overlaps
|
|
'''
|
|
product = sum(imgs[0][:,SIZE-overlaps:].astype(np.uint32)*imgs[1][:,:overlaps].astype(np.uint32))
|
|
product += sum(imgs[2][:,SIZE-overlaps:].astype(np.uint32)*imgs[3][:,:overlaps].astype(np.uint32))
|
|
product = sum(product) / len(product)
|
|
maxProduct = product
|
|
tmp = maxProduct
|
|
maxProduct = 0
|
|
product = sum(imgs[0][SIZE-overlaps:, :].astype(np.uint32)*imgs[2][:overlaps,:].astype(np.uint32))
|
|
product += sum(imgs[1][SIZE-overlaps:, :].astype(np.uint32)*imgs[3][:overlaps,:].astype(np.uint32))
|
|
product = sum(product) / len(product)
|
|
maxProduct = product
|
|
maxProduct = (tmp + maxProduct)/2
|
|
'''
|
|
|
|
#if maxProduct > PRODUCTION_THRESHOLD:
|
|
if True:
|
|
tmp = np.zeros((SIZE, SIZE*2-overlap_w), dtype=np.uint16)
|
|
tmp[:, :SIZE] = imgs[0]
|
|
tmp[:, -SIZE:] += imgs[1]
|
|
tmp[:, (SIZE-overlap_w): SIZE] = tmp[:, (SIZE-overlap_w): SIZE]/2
|
|
|
|
tmp2 = np.zeros((SIZE, SIZE*2-overlap_w), dtype=np.uint16)
|
|
tmp2[:, :SIZE] = imgs[2]
|
|
tmp2[:, -SIZE:] += imgs[3]
|
|
tmp2[:, (SIZE-overlap_w): SIZE] = tmp2[:, (SIZE-overlap_w): SIZE]/2
|
|
|
|
merge = np.zeros((SIZE*2-overlap_h, SIZE*2-overlap_w), dtype=np.uint16)
|
|
merge[:SIZE, :] = tmp
|
|
merge[-SIZE:, :] += tmp2
|
|
merge[(SIZE-overlap_h):SIZE, :] = merge[(SIZE-overlap_h):SIZE, :]/2
|
|
offset_w = int(overlap_w/2)
|
|
offset_h = int(overlap_h/2)
|
|
out[SIZE*2+offset_h:SIZE*4-overlap_h+offset_h, offset_w: SIZE*2-overlap_w+offset_w] = merge
|
|
'''
|
|
position = [0,0]
|
|
rows,cols = merge.shape
|
|
|
|
for i in range(rows):
|
|
for j in range(cols):
|
|
position[0] += i*merge[i][j]
|
|
position[1] += j*merge[i][j]
|
|
position[0] /= sum(sum(merge))
|
|
position[1] /= sum(sum(merge))
|
|
|
|
pos_w = 1.17*position[0] #distanceBetweenSensors_w/(SIZE-overlap_w)*position[0]
|
|
pos_h = 1.17*position[1] #distanceBetweenSensors_h/(SIZE-overlap_h)*position[1]
|
|
if not hasPos:
|
|
startPos = [pos_w, pos_h]
|
|
sp = position
|
|
path = []
|
|
truePath = []
|
|
times = []
|
|
startTime = frames[0].time
|
|
hasPos = True
|
|
if not innerHasPos and pos_w >= 16 and pos_w <= 109 and pos_h >= 16 and pos_h <= 109:
|
|
innerStartPos = [pos_w, pos_h]
|
|
innerStartTime = frames[0].time
|
|
innerHasPos = True
|
|
|
|
if pos_w >= 16 and pos_w <= 109 and pos_h >= 16 and pos_h <= 109:
|
|
innerEndPos = [pos_w, pos_h]
|
|
innerEndTime = frames[0].time
|
|
elif innerHasPos:
|
|
if innerEndTime - innerStartTime > 0:
|
|
print (innerStartPos, innerEndPos)
|
|
print ('inner speed:', ((innerEndPos[0]-innerStartPos[0])**2+(innerEndPos[1]-innerStartPos[1])**2)**0.5/(innerEndTime - innerStartTime))
|
|
print ('time:', innerEndTime-innerStartTime)
|
|
innerHasPos = False
|
|
|
|
endPos = [pos_w, pos_h]
|
|
endTime = frames[0].time
|
|
path.append(position)
|
|
truePath.append(endPos)
|
|
times.append(frames[0].time)
|
|
'''
|
|
elif hasPos:
|
|
if endTime - startTime > 0:
|
|
print (startPos, endPos)
|
|
print ('speed:', ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime))
|
|
print ('time:', endTime-startTime)
|
|
if innerHasPos and innerEndTime - innerStartTime > 0:
|
|
print (innerStartPos, innerEndPos)
|
|
print ('inner speed:', ((innerEndPos[0]-innerStartPos[0])**2+(innerEndPos[1]-innerStartPos[1])**2)**0.5/(innerEndTime - innerStartTime))
|
|
print ('time:', innerEndTime-innerStartTime)
|
|
hasPos = False
|
|
innerHasPos = False
|
|
out = out.astype(np.uint8)
|
|
out = exponential(out, EXPONENTAL_VALUE)
|
|
|
|
out = cv2.cvtColor(out,cv2.COLOR_GRAY2BGR)
|
|
'''
|
|
if endTime - startTime > MIN_EXIST_TIME:
|
|
speed = ((endPos[0]-startPos[0])**2+(endPos[1]-startPos[1])**2)**0.5/(endTime - startTime)
|
|
cv2.putText(out, f'{speed:.2f}',
|
|
(0, SIZE*2),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
|
|
speed = ((truePath[-1][0]-truePath[-2][0])**2+(truePath[-1][1]-truePath[-2][1])**2)**0.5/(times[-1] - times[-2])
|
|
cv2.putText(out, f'{speed:.2f}',
|
|
(0, SIZE*2+30),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
|
|
if maxProduct > PRODUCTION_THRESHOLD:
|
|
cv2.circle(out, (offset_w+int(position[1]), SIZE*2+offset_h+int(position[0])), 10, (255,0,0), 5)
|
|
cv2.circle(out, (offset_w+int(sp[1]), SIZE*2+offset_h+int(sp[0])), 10, (0,255,0), 5)
|
|
for i in range(len(path)-1):
|
|
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))
|
|
cv2.line
|
|
lastFrame = out[SIZE*2:,:]
|
|
hasLastFrame = True
|
|
elif hasLastFrame:
|
|
out[SIZE*2:,:] = lastFrame
|
|
'''
|
|
cv2.imshow('sample', out)
|
|
videoWriter.write(out)
|
|
|
|
key = cv2.waitKey(1)
|
|
if key == ord('q'):
|
|
break
|
|
|
|
videoWriter.release()
|
|
cv2.destroyAllWindows()
|