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.
241 lines
8.7 KiB
241 lines
8.7 KiB
import cv2
|
|
import serial
|
|
import threading
|
|
import time
|
|
|
|
class Frame():
|
|
def __init__(self, time, data):
|
|
self.time = time
|
|
self.data = data
|
|
|
|
class GridEye():
|
|
def __init__(self, serialPort, baudrate):
|
|
self.port = serial.Serial(serialPort, baudrate)
|
|
self.frame = None
|
|
self.reading = True
|
|
self.thread = threading.Thread(target = self.reader)
|
|
self.thread.setDaemon(True)
|
|
self.lock = threading.Lock()
|
|
|
|
def start(self):
|
|
self.port.reset_input_buffer()
|
|
self.thread.start()
|
|
|
|
def stop(self):
|
|
self.reading = False
|
|
self.thread.join()
|
|
|
|
def reader(self):
|
|
data = []
|
|
data_time = 0;
|
|
while (self.reading):
|
|
line = b''
|
|
while (self.reading):
|
|
c = self.port.read()
|
|
if c == b'\r':
|
|
c = self.port.read()
|
|
break
|
|
if c == b'\n':
|
|
break
|
|
line += c
|
|
#line = self.port.readline()#.decode('utf-8')
|
|
# if line:
|
|
# print (line)
|
|
# time.sleep(0.01)
|
|
# if self.port.in_waiting > 0:
|
|
# print (self.port.in_waiting)
|
|
if b'#' in line:
|
|
if len(data) == 8:
|
|
#print (data)
|
|
self.lock.acquire()
|
|
self.frame = Frame(data_time, data)
|
|
self.lock.release()
|
|
else:
|
|
print ('something wrong', len(data))
|
|
data_time = time.time()
|
|
data = []
|
|
else:
|
|
try:
|
|
row = [float(x) for x in line.split()]
|
|
if len(row) == 8:
|
|
data.append(row)
|
|
except ValueError as e:
|
|
print ('error', e)
|
|
data_time = time.time()
|
|
data = []
|
|
if len(data) > 8:
|
|
data.pop(0)
|
|
|
|
class Distance():
|
|
def __init__(self, serialPort, baudrate):
|
|
self.port = serial.Serial(serialPort, baudrate)
|
|
self.distance = 200
|
|
self.reading = True
|
|
self.thread = threading.Thread(target = self.reader)
|
|
self.thread.setDaemon(True)
|
|
self.lock = threading.Lock()
|
|
|
|
def start(self):
|
|
self.port.reset_input_buffer()
|
|
self.thread.start()
|
|
|
|
def stop(self):
|
|
self.reading = False
|
|
self.thread.join()
|
|
|
|
def reader(self):
|
|
while (self.reading):
|
|
line = b''
|
|
while (self.reading):
|
|
c = self.port.read()
|
|
if c == b'\r':
|
|
c = self.port.read()
|
|
break
|
|
if c == b'\n':
|
|
break
|
|
line += c
|
|
if b'Distance' in line:
|
|
try:
|
|
dist = float(line.decode('utf-8').split(':')[1])
|
|
print (dist)
|
|
if dist > 200.0:
|
|
dist = 200.0
|
|
self.lock.acquire()
|
|
self.distance = dist
|
|
self.lock.release()
|
|
except ValueError as e:
|
|
print ('error', e)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
import cv2
|
|
import numpy as np
|
|
import math
|
|
def exponential(img, value):
|
|
tmp = cv2.pow(img.astype(np.double), value)*(255.0/(255.0**value))
|
|
return tmp.astype(np.uint8)
|
|
|
|
SIZE = 128
|
|
overlap = 120
|
|
AVERAGE_FRAME = 10
|
|
distanceBetweenSensors = 7.7 #cm
|
|
distance2Object = 60.0 #cm
|
|
ADJUST_BACK = 5
|
|
EXPONENTAL_VALUE = 0.4
|
|
|
|
offset = (distanceBetweenSensors / (2*distance2Object*math.tan(30.0/180.0*math.pi))) * SIZE
|
|
overlap = int(SIZE - offset)
|
|
print (overlap)
|
|
grideye = GridEye('COM15', 115200)
|
|
grideye.start()
|
|
grideye2 = GridEye('COM17', 115200)
|
|
grideye2.start()
|
|
|
|
distanceSensor = Distance('COM18', 9600)
|
|
distanceSensor.start()
|
|
|
|
fourcc = cv2.VideoWriter_fourcc(*'XVID')
|
|
videoWriter = cv2.VideoWriter('output.avi', fourcc, 10.0, (SIZE*2,SIZE*3))
|
|
cv2.imshow('sample', np.zeros((SIZE*3,SIZE*2), np.uint8))
|
|
aver1 = np.zeros((SIZE,SIZE), np.uint16)
|
|
aver2 = np.zeros((SIZE,SIZE), np.uint16)
|
|
cnt = 0
|
|
while True:
|
|
if grideye.frame and grideye2.frame:
|
|
grideye.lock.acquire()
|
|
grideye2.lock.acquire()
|
|
distanceSensor.lock.acquire()
|
|
frame = grideye.frame
|
|
grideye.frame = None
|
|
frame2 = grideye2.frame
|
|
grideye2.frame = None
|
|
# frame2 = frame
|
|
distance2Object = distanceSensor.distance
|
|
distanceSensor.lock.release()
|
|
grideye2.lock.release()
|
|
grideye.lock.release()
|
|
|
|
img = (np.array(frame.data)-15)*10
|
|
img = cv2.resize(img.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_CUBIC) # INTER_LINEAR, INTER_CUBIC
|
|
img2 = (np.array(frame2.data)-15)*10
|
|
img2 = cv2.resize(img2.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_CUBIC)
|
|
|
|
if cnt < AVERAGE_FRAME:
|
|
cnt += 1
|
|
aver1 += img
|
|
aver2 += img2
|
|
if cnt == AVERAGE_FRAME:
|
|
aver1 = aver1/AVERAGE_FRAME
|
|
aver1 = aver1.astype(np.uint8)
|
|
aver1 += ADJUST_BACK
|
|
aver2 = aver2/AVERAGE_FRAME
|
|
aver2 = aver2.astype(np.uint8)
|
|
aver2 += ADJUST_BACK
|
|
continue
|
|
img = cv2.subtract(img, aver1)
|
|
img2 = cv2.subtract(img2, aver2)
|
|
|
|
out = np.full((SIZE*3, SIZE*2), 255, dtype=np.uint16)
|
|
out[:SIZE, :SIZE] = img
|
|
out[:SIZE, SIZE:] = img2
|
|
|
|
|
|
overlap = int(SIZE - (distanceBetweenSensors / (2*distance2Object*math.tan(30.0/180.0*math.pi))) * SIZE)
|
|
offset = int(overlap/2)
|
|
# tmp = cv2.resize(img.astype(np.uint8), (SIZE*2-overlap, SIZE))
|
|
# tmp.astype(np.uint16)
|
|
tmp = np.zeros((SIZE, SIZE*2-overlap), dtype=np.uint16)
|
|
tmp[:, :SIZE] = img
|
|
tmp[:, -SIZE:] += img2
|
|
tmp[:, (SIZE-overlap): SIZE] = tmp[:, (SIZE-overlap): SIZE]/2
|
|
tmp = exponential(tmp, EXPONENTAL_VALUE)
|
|
out[SIZE:SIZE*2, offset: SIZE*2-overlap+offset] = tmp
|
|
# out[SIZE:SIZE*2, offset:SIZE+offset] = img
|
|
# out[SIZE:SIZE*2, (SIZE-overlap)+offset:SIZE+offset] += img2[:,:overlap]
|
|
# out[SIZE:SIZE*2, (SIZE-overlap)+offset:SIZE+offset] = out[SIZE:SIZE*2, (SIZE-overlap)+offset:SIZE+offset]/2
|
|
# out[SIZE:SIZE*2, SIZE+offset:SIZE+(SIZE-overlap)+offset] = img2[:,overlap:SIZE]
|
|
|
|
|
|
maxProduct = 0
|
|
overlap2 = 0
|
|
for i in range(80, 128):
|
|
product = sum(img[:,SIZE-i:].astype(np.uint32)*img2[:,:i].astype(np.uint32))
|
|
product = sum(product) / len(product)
|
|
if product > maxProduct:
|
|
maxProduct = product
|
|
overlap2 = i
|
|
|
|
|
|
|
|
offset = int(overlap2/2)
|
|
tmp = np.zeros((SIZE, SIZE*2-overlap2), dtype=np.uint16)
|
|
tmp[:, :SIZE] = img
|
|
tmp[:, -SIZE:] += img2
|
|
tmp[:, (SIZE-overlap2): SIZE] = tmp[:, (SIZE-overlap2): SIZE]/2
|
|
tmp = exponential(tmp, EXPONENTAL_VALUE)
|
|
out[SIZE*2:, offset: SIZE*2-overlap2+offset] = tmp
|
|
|
|
|
|
# out[SIZE*2:, offset:SIZE+offset] = img
|
|
# out[SIZE*2:, (SIZE-overlap2)+offset:SIZE+offset] += img2[:,:overlap2]
|
|
# out[SIZE*2:, (SIZE-overlap2)+offset:SIZE+offset] = out[SIZE*2:, (SIZE-overlap2)+offset:SIZE+offset]/2
|
|
# out[SIZE*2:, SIZE+offset:SIZE+(SIZE-overlap2)+offset] = img2[:,overlap2:SIZE]
|
|
out = out.astype(np.uint8)
|
|
|
|
cv2.imshow('sample', out)
|
|
videoWriter.write(cv2.cvtColor(out,cv2.COLOR_GRAY2BGR))
|
|
key = cv2.waitKey(1)
|
|
if key == ord('q'):
|
|
break
|
|
elif key == ord('a'):
|
|
overlap += 1
|
|
elif key == ord('d'):
|
|
overlap -= 1
|
|
elif key == ord('c'):
|
|
cv2.imwrite('out.jpg', out)
|
|
time.sleep(0.001)
|
|
grideye.stop()
|
|
grideye2.stop()
|
|
videoWriter.release()
|
|
cv2.destroyAllWindows()
|
|
|