I tried the following code as main.py on Nicla Vision Board
import sensor, ustruct, pyb, time, image
usb_vcp = pyb.USB_VCP()
usb_vcp.setinterrupt(-1)
green_led = pyb.LED(2)
blue_led = pyb.LED(3)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.skip_frames(time=2000)
originList = [(207, 282), (204, 194), (202, 106), (200, 19), (161, 238), (159, 151), (158, 64), (118, 283), (117, 195), (115, 108), (113, 21), (75, 240), (72, 152), (71, 66), (30, 197), (30, 285), (27, 109), (25, 21)]
extra_fb = sensor.alloc_extra_fb(sensor.width(), sensor.height(), sensor.RGB565)
extra_fb.replace(sensor.snapshot())
while True:
command = usb_vcp.recv(4, timeout=5000)
if command == b'snap':
image = sensor.snapshot()
marker_dx = []
marker_dy = []
marker_response = []
for o in originList:
o_x = o[1]
o_y = o[0]
x = o_x - 32
y = o_y - 32
displacement = extra_fb.find_displacement(img, roi = (x, y, BLOCK_W, BLOCK_H), template_roi = (x, y, BLOCK_W, BLOCK_H))
d_x = int(displacement.x_translation() * 5) / 5.0
d_y = int(displacement.y_translation() * 5) / 5.0
# marker_origin.append(o)
marker_dx.append(d_x)
marker_dy.append(d_y)
marker_response.append(displacement.response())
list_final = marker_dx + marker_dy + marker_response
# list_final = [0,0]
image1 = image.compress()
usb_vcp.send(ustruct.pack('<L', image1.size()))
usb_vcp.send(image1)
command_d = usb_vcp.recv(4, timeout=5000)
if command_d == b'list':
blue_led.on()
s = ustruct.pack('54d', *list_final)
usb_vcp.send(s)
blue_led.off()
It doesn’t communicate as it encounters the find_displacemnt function inside the code.
Could you help to find what the bug is here? If I comment out the part that calculates optical flow, everything works fine.
The python script that use on my computer is as follows:
import io
import struct
import sys
import numpy as np
import serial
from PIL import Image as PILImage
import matplotlib.pyplot as plt
import cv2
class OpenMVCamRead:
def __init__(self, device='COM3'):
self.port = serial.Serial(device, baudrate=115200,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
xonxoff=False, rtscts=False,
stopbits=serial.STOPBITS_ONE,
timeout=None, dsrdtr=True)
self.port.setDTR(True)
self.port.reset_input_buffer()
self.port.reset_output_buffer()
self.initArea = np.zeros((3,4), dtype=np.float64)
self.init0d = np.zeros((3,4), dtype=np.float64)
self.init45d = np.zeros((3,4), dtype=np.float64)
self.init90d = np.zeros((3,4), dtype=np.float64)
self.init135d = np.zeros((3,4), dtype=np.float64)
self.initPlusCross = np.zeros((3,4), dtype = np.float64)
self.folder_name = "dataset"
self.originList = [(207, 282), (204, 194), (202, 106), (200, 19), (161, 238), (159, 151), (158, 64), (118, 283), (117, 195), (115, 108), (113, 21), (75, 240), (72, 152), (71, 66), (30, 197), (30, 285), (27, 109), (25, 21)]
def readFromNicla(self, count):
snap = 'snap'
if sys.version_info[0] >= 3:
snap = snap.encode('utf-8')
self.port.write(snap)
self.port.flush()
print("SNAP")
size = struct.unpack('<L', self.port.read(4))[0]
image_data = self.port.read(size)
image = np.array(PILImage.open(io.BytesIO(image_data)))
print("IMAGE DATA")
list_ = 'list'
if sys.version_info[0] >= 3:
list_ = list_.encode('utf-8')
self.port.write(list_)
self.port.flush()
print("LIST")
list_ = struct.unpack('54d', self.port.read(432))
print(count)
# list_ = [1,1]
return image, list_
def plot_mesh(self, image):
block_h = 64
block_w = 64
val = 32
for o in self.originList:
o_x = o[1]
o_y = o[0]
image = cv2.circle(image, (o_x,o_y), 1, (0,0,0), 1)
x = o_x - val
y = o_y - val
image = cv2.rectangle(image, (x, y), (x+block_w, y+block_h), (255,0,0), 1)
return image
def interpretAndStore(self, image, list_, imgCount):
if imgCount < 100:
count = "0" + str(imgCount)
else:
count = str(imgCount)
# store data from nicla
marker_dx = (np.array(list_[0:18]))
marker_dy = (np.array(list_[18:36]))
marker_confidence = (np.array(list_[36:54]))
# store original img
name_img = "..\\" + self.folder_name + "\\" + count + "img.png"
cv2.imwrite(name_img, cv2.cvtColor(image, cv2.COLOR_BGR2RGB))
# review trhe field
figure1, axis1 = plt.subplots(1, 1)
image_field = self.plot_mesh(image)
axis1.imshow(image_field,cmap='gray')
plt.savefig('..\\'+ self.folder_name +'\\' + count + 'field.png')
# find correspondence according to our algorithm
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
blur = cv2.GaussianBlur(gray, (0,0), sigmaX=33, sigmaY=33)
divide = cv2.divide(gray, blur, scale=255)
# cv2.imshow(divide)
ret, frame = cv2.threshold(divide, 120, 255, cv2.THRESH_BINARY)
# cv2.imshow(frame)
kernel1 = np.ones((3,3), np.uint8)
kernel2 = np.ones((2,2), np.uint8)
iter1 = 1
iter2 = 1
img_erosion = cv2.erode(frame, kernel1, iterations=iter1)
img_dilation = cv2.dilate(img_erosion, kernel2, iterations=iter2)
# cv2.imshow(img_dilation)
me_marker_dx = []
me_marker_dy = []
for o in self.originList:
ind = np.array(np.where(img_dilation[o[0]-32:o[0]+32, o[1]-32:o[1]+32]==0))
dy = (np.mean(o[0]-32 + ind[0])) - o[0]
dx = (np.mean(o[1]-32 + ind[1])) - o[1]
me_marker_dx.append(dx)
me_marker_dy.append(dy)
# dump displacement according to nicla and mine
print("\nnicla displacement x:\n", marker_dx)
print("\nnicla displacement y:\n", marker_dy)
print("\nme displacement x:\n", me_marker_dx)
print("\nme displacement x:\n", me_marker_dx)
# plot nicla
# plot me
if __name__ == '__main__':
instance = OpenMVCamRead()
folder_name = input("Write the folder name")
instance.folder_name = folder_name
imgCount = 1
while (True):
image, list_ = instance.readFromNicla(imgCount)
instance.interpretAndStore(image, list_, imgCount)
imgCount+=1