import readGridEyeData
|
|
|
|
import sys
|
|
import numpy as np
|
|
import cv2
|
|
|
|
SIZE = 8
|
|
visible_size = 128
|
|
distanceBetweenSensors_w = 2.6 #cm
|
|
distanceBetweenSensors_h = 2.6 #cm
|
|
distance2Object = 60.0 #cm
|
|
OFFSET = ((0, 0), ())
|
|
|
|
TEMP_SCALE = 20
|
|
RED = (0, 0, 255)
|
|
GREEN = (0, 255, 0)
|
|
BLUE = (255, 0, 0)
|
|
|
|
background = readGridEyeData.GridEyeData(sys.argv[1])
|
|
|
|
background_frames = [np.zeros((SIZE,SIZE)), np.zeros((SIZE,SIZE)),
|
|
np.zeros((SIZE,SIZE)), np.zeros((SIZE,SIZE))]
|
|
cnt = 1
|
|
Horizon_array = np.array([[x for x in range(8)] for _ in range(8)])
|
|
Vertical_array = np.array([[x for _ in range(8)] for x in range(8)])
|
|
|
|
# Gets background of each pixel.
|
|
while background.readFrame():
|
|
frames = background.frames
|
|
for i in range(4):
|
|
background_frames[i] += np.array(frames[i].data)
|
|
cnt += 1
|
|
|
|
for i in range(4):
|
|
background_frames[i]/=cnt
|
|
|
|
# Reads data from object frames.
|
|
objects = readGridEyeData.GridEyeData(sys.argv[2])
|
|
|
|
while objects.readFrame():
|
|
frames = objects.frames
|
|
imgs = []
|
|
heat_points = []
|
|
|
|
# Subtracts background and scale the value.
|
|
for i in range(4):
|
|
frame = np.array(frames[i].data) - background_frames[i]
|
|
frame[frame < 0] = 0.0
|
|
frame *= TEMP_SCALE
|
|
frame[frame > 255] = 255
|
|
imgs.append(frame.astype(np.uint8))
|
|
x = int(sum(sum(frame * Horizon_array)) / sum(sum(frame)) / (SIZE - 1) *
|
|
(visible_size - 1))
|
|
y = int(sum(sum(frame * Vertical_array)) / sum(sum(frame)) / (SIZE - 1) *
|
|
(visible_size - 1))
|
|
heat_points.append((x,y))
|
|
print (x, y)
|
|
all_img = np.concatenate((np.concatenate((imgs[0], imgs[1]), axis = 1),
|
|
np.concatenate((imgs[2], imgs[3]), axis = 1)),
|
|
axis = 0)
|
|
all_img = cv2.cvtColor(all_img, cv2.COLOR_GRAY2RGB)
|
|
all_img = cv2.resize(all_img, (visible_size*2, visible_size*2),
|
|
interpolation = cv2.INTER_NEAREST)
|
|
cv2.circle(all_img, heat_points[0], 5, RED, -1)
|
|
cv2.circle(all_img, (heat_points[1][0]+visible_size, heat_points[1][1]), 5, RED, -1)
|
|
|
|
cv2.imshow('sample', all_img)
|
|
|
|
|
|
while cv2.getWindowProperty('sample', 0) >= 0:
|
|
key = cv2.waitKey(50)
|
|
if key >= 0:
|
|
break
|