def onStartButton(self):
if self._buttonFlag:
self._buttonFlag = True
# self.ui.pushButton.setText('Start')
else:
self._buttonFlag = True
# self.ui.pushButton.setText('Stop')
try:
self.handle, self.info = sc.open_sensel()
except OSError:
sc.close_sensel(self.handle, self.frame)
# Open Morph
self.handle, self.info = sc.open_sensel()
# Initalize frame
self.frame = sc.init_frame(self.handle, baseline=0)
# update using timer
# self.timer = QtCore.QTimer()
# self.timer.timeout.connect(self.updateData)
self.timer.start(self.interval)
def updateData(self):
# scan image from the device
try:
self.f_image = sc.scan_frames(self.handle, self.frame, self.info)
except(UnboundLocalError):
try:
sc.close_sensel(self.handle, self.frame)
# Open Morph
self.handle, self.i = sc.open_sensel()
# Initalize frame
self.frame = sc.init_frame(self.handle, baseline=0)
self.f_image = sc.scan_frames(self.handle, self.frame, self.info)
except(UnboundLocalError):
self.f_image = np.zeros((self.rows, self.cols))
# update blob information
self.blobs = self.BlobTracker.update(self.f_image)
# update marker information
self.MarkerTracker.update(self.f_image, self.blobs)
if len(self.MarkerTracker.markers) > 0:
# print('markerID: ' + str(self.markers[0].ID))
# print('markerForce: ' + str(self.markers[0].sumForce()))
# print('markerVectorForce: ' + str(self.markers[0].vectorForce()))
if not self._popflag:
self.SIGNALS.OPEN.emit()
else:
if self._popflag:
self.SIGNALS.CLOSE.emit()
# send marker parameters to GUI
self.sendMarkerParameters()
'''
# retrieve peak coordinates from the peak image
self.peaks = forcestamp.findPeakCoord(self.f_image_peaks)
# exclude marker areas from the peak coords
self.f_image_peaks_excluded = deepcopy(self.f_image_peaks)
for mkr in self.markers:
self.f_image_peaks_excluded = forcestamp.excludeMarkerPeaks(self.f_image_peaks_excluded, (mkr.pos_y, mkr.pos_x), mkr.radius)
self.peaks_excluded = forcestamp.findPeakCoord(self.f_image_peaks_excluded)
self.peaks_excluded = forcestamp.findSubpixelPeaks(self.peaks_excluded, self.f_image)
self.peaks_force = []
for pk in self.peaks_excluded:
self.peaks_force.append(np.sum(forcestamp.cropImage(self.f_image, pk, radius=3, margin=1)))
# print(self.peaks_excluded)
# print(self.peaks_force)
# print(self.peaks_excluded)
if len(self.peaks_excluded) > 0:
self.sendOSC_coords(self.peaks_excluded, self.peaks_force)
'''
# prepare a image copy for display
f_image_show = deepcopy(self.f_image)
if np.max(f_image_show) > 0:
f_image_show = f_image_show / np.max(f_image_show) * 255
f_image_show = cv2.cvtColor(f_image_show.astype(np.uint8), cv2.COLOR_GRAY2RGB)
# draw peaks
for b in self.blobs:
cv2.circle(
f_image_show,
(np.int(b.cx), np.int(b.cy)),
0,
(0, 255, 255)
)
# set image for display
self.img.setImage(np.rot90(f_image_show, 3), autoLevels=True, levels=(0, 50))
# self.calculateFPS()
QtGui.QApplication.processEvents()
# add image item view1.addItem(img1) view2.addItem(img2) view3.addItem(img3) view4.addItem(img4) # Set initial view bounds view1.setRange(QtCore.QRectF(0, 0, rows, cols)) view2.setRange(QtCore.QRectF(0, 0, rows, cols)) view3.setRange(QtCore.QRectF(0, 0, rows, cols)) view4.setRange(QtCore.QRectF(0, 0, 60, 60)) layout.addWidget(grview, 0, 0) # Sensel initialization handle, info = sc.open_sensel() # Initalize frame frame = sc.init_frame(handle, detail=0, baseline=0) # update interval interval = 0 # miliseconds lastTime = time() fps = None BlobTracker = forcestamp.TrackBlobs() marker_radii = [55 / 2 / 1.25, 17 / 1.25, 20] MarkerTracker = forcestamp.TrackMarkers(radii=marker_radii)
def update():
global lastTime, fps, info, handle, frame
try:
f_image = sc.scan_frames(handle, frame, info)
except UnboundLocalError:
sc.close_sensel(handle, frame)
# Sensel initialization
handle, info = sc.open_sensel()
# Initalize frame
frame = sc.init_frame(handle, detail=0, baseline=0)
f_image = sc.scan_frames(handle, frame, info)
# print(np.max(f_image))
# find blobs from the image
blobs, contours, hierarchy, areas, cx, cy, forces, f_image_thre = forcestamp.detectBlobs(f_image, areaThreshold=1000)
# print(contours)
# update blob information
blobs = BlobTracker.update(f_image)
# update marker information
MarkerTracker.update(f_image, blobs)
# prepare image to show
f_image_show = copy.deepcopy(f_image)
if np.max(f_image_show) > 0:
f_image_show = f_image_show / np.max(f_image_show) * 255
f_image_show = cv2.cvtColor(f_image_show.astype(np.uint8),
cv2.COLOR_GRAY2RGB
)
# f_image_thre = np.zeros((cols, rows), dtype=np.uint8)
# if np.max(f_image) > 0:
# f_image_thre = forcestamp.findLocalPeaks(f_image, threshold=0.2).astype(np.uint8)
# peak_coords = forcestamp.findPeakCoord(f_image_peaks)
# f_image_thre = forcestamp.findSubpixelPeaks(peak_coords, f_image)
# # draw contours
# if len(blobs) > 0:
# for b in blobs:
# cv2.drawContours(
# f_image_show,
# [b.contour],
# 0,
# (0, 255, 0),
# 1
# )
# display force
# if len(blobs) > 0:
# for b in blobs:
# font = cv2.FONT_HERSHEY_SIMPLEX
# cv2.putText(
# f_image_show,
# '%0.1f' % b.force,
# (int(b.cx) + 5, int(b.cy) + 10),
# font,
# 0.3, # font size
# (0, 255, 0),
# 1,
# cv2.LINE_AA
# )
# # display appeared time
# if len(blobs) > 0:
# for b in blobs:
# font = cv2.FONT_HERSHEY_SIMPLEX
# cv2.putText(
# f_image_show,
# '%5.2f' % b.t_appeared,
# (int(b.cx), int(b.cy) + 10),
# font,
# 0.3, # font size
# (0, 255, 255),
# 1,
# cv2.LINE_AA
# )
# display ID
# if len(blobs) > 0:
# for b in blobs:
# font = cv2.FONT_HERSHEY_SIMPLEX
# cv2.putText(
# f_image_show,
# 'ID: %d' % b.ID,
# (int(b.cx) + 5, int(b.cy) + 0),
# font,
# 0.3, # font size
# (255, 255, 255),
# 1,
# cv2.LINE_AA
# )
# show peaks
if len(blobs) > 0:
for b in blobs:
cv2.circle(
f_image_show,
(np.int(b.cx), np.int(b.cy)),
0,
(255, 0, 0)
)
# show marker center
for mkr in MarkerTracker.markers:
cv2.circle(
f_image_show,
(np.int(mkr.pos_x), np.int(mkr.pos_y)),
3,
(0, 255, 0)
)
cv2.line(
f_image_show,
(np.int(mkr.pos_x), np.int(mkr.pos_y)),
(np.int(mkr.pos_x + mkr.radius * np.sin(mkr.rot)), np.int(mkr.pos_y + mkr.radius * np.cos(mkr.rot))),
(255, 0, 0),
1
)
cv2.line(
f_image_show,
(np.int(mkr.pos_x), np.int(mkr.pos_y)),
(np.int(mkr.pos_x + 1 * mkr.cof_x), np.int(mkr.pos_y - 1 * mkr.cof_y)),
(255, 255, 0),
1
)
# show marker blobs
for mkr in MarkerTracker.markers:
for b in mkr.blobs:
cv2.circle(
f_image_show,
(np.int(b.cx), np.int(b.cy)),
0,
(0, 255, 0)
)
# display marker radius
for mkr in MarkerTracker.markers:
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(
f_image_show,
'ID: %d' % mkr.ID,
(int(mkr.pos_x) + 5, int(mkr.pos_y) + 0),
font,
0.3, # font size
(255, 255, 255),
1,
cv2.LINE_AA
)
cv2.putText(
f_image_show,
'%d' % mkr.force,
(int(mkr.pos_x) + 5, int(mkr.pos_y) + 10),
font,
0.3, # font size
(0, 255, 0),
1,
cv2.LINE_AA
)
for b in mkr.blobs:
cv2.putText(
f_image_show,
'%d' % b.slot,
(int(b.c[0]) + 0, int(b.c[1]) + 3),
font,
0.3, # font size
(0, 255, 255),
1,
cv2.LINE_AA
)
# cv2.putText(
# f_image_show,
# '%d' % b.ID,
# (int(b.c[0]) + 0, int(b.c[1]) - 3),
# font,
# 0.2, # font size
# (255, 255, 255),
# 1,
# cv2.LINE_AA
# )
for mkr in MarkerTracker.markers:
# print(mkr.cof_x, mkr.cof_y)
# print(mkr.code)
# print(mkr.phaseError)
# IDs = []
# for b in mkr.blobs:
# IDs.append(b.ID)
# print(IDs)
# print('------------------')
# print(mkr.phaseError)
# print(mkr.code)
# print(mkr.codeword)
# print(np.rad2deg([b.phase for b in mkr.blobs]))
# print(np.rad2deg(mkr.rot))
print(np.sqrt(mkr.cof_x ** 2 + mkr.cof_y ** 2))
img4.setImage(np.rot90(mkr.markerImg, 3), autoLevels=True, levels=(0, 80))
# img1.setImage(np.rot90(f_image, 3), autoLevels=True, levels=(0, 50))
# img1.setImage(np.rot90(f_image_peaks, 3), autoLevels=True, levels=(0, 50))
img1.setImage(np.rot90(f_image, 3), autoLevels=False, levels=(0, 80))
img2.setImage(np.rot90(f_image_show, 3), autoLevels=True, levels=(0, 80))
img3.setImage(np.rot90(f_image_thre, 3), autoLevels=True, levels=(0, 80))
# img3.setImage(np.rot90(unknown, 3), autoLevels=True, levels=(0, 80))
now = time()
dt = now - lastTime
lastTime = now
if fps is None:
fps = 1.0 / dt
else:
s = np.clip(dt * 3., 0, 1)
fps = fps * (1 - s) + (1.0 / dt) * s
# print('%0.2f fps' % fps)
QtGui.QApplication.processEvents()
def update():
global lastTime, fps, info, handle, frame, f_image, do_save
try:
f_image = sc.scan_frames(handle, frame, info)
except UnboundLocalError:
sc.close_sensel(handle, frame)
# Sensel initialization
handle, info = sc.open_sensel()
# Initalize frame
frame = sc.init_frame(handle, detail=0, baseline=0)
f_image = sc.scan_frames(handle, frame, info)
# print(np.max(f_image))
# prepare image to show
f_image_show = copy.deepcopy(f_image)
if np.max(f_image_show) > 0:
f_image_show = f_image_show / np.max(f_image_show) * 255
f_image_show = cv2.cvtColor(f_image_show.astype(np.uint8),
cv2.COLOR_GRAY2RGB)
# # find blobs from the image
# blobs, contours, hierarchy, areas, cx, cy, forces, f_image_thre, f_image_interp = pneumodule.detectBlobs(f_image, areaThreshold=1000, interp=interp)
# print(contours)
# interpolate image
f_image_interp = cv2.resize(
f_image, (f_image.shape[1] * interp, f_image.shape[0] * interp),
interpolation=cv2.INTER_LANCZOS4)
if np.max(f_image_interp) > 0:
f_image_interp = f_image_interp / np.max(f_image_interp) * 255
f_image_interp = cv2.cvtColor(f_image_interp.astype(np.uint8),
cv2.COLOR_GRAY2RGB)
# update blob information
blobs = BlobTracker.update(f_image)
# update marker information
MarkerTracker.update(blobs, f_image)
if len(MarkerTracker.markers) > 0:
marker_num = 0
for mkr in MarkerTracker.markers:
# print('life time:', mkr.lifetime)
mkr_center = (np.int(mkr.pos_x * interp),
np.int(mkr.pos_y * interp))
# cv2.circle(
# f_image_interp,
# mkr_center,
# 30,
# (255, 255, 255)
# )
# cv2.line(f_image_interp, mkr_center, (int(mkr_center[0] + mkr.rot[0] * 40), int(mkr_center[1] + mkr.rot[1] * 40)), (255, 255, 255), 3)
cv2.arrowedLine(
f_image_interp,
(int(mkr_center[0] +
mkr.rot[0] * mkr.marker_size / 2 * interp),
int(mkr_center[1] +
mkr.rot[1] * mkr.marker_size / 2 * interp)),
(int(mkr_center[0] +
mkr.rot[0] * mkr.marker_size / 2 * interp * 1.5),
int(mkr_center[1] +
mkr.rot[1] * mkr.marker_size / 2 * interp * 1.5)),
(255, 255, 255),
thickness=5,
tipLength=0.5)
# for b in mkr.blobs:
# if b is not None:
# cv2.circle(
# f_image_interp,
# (np.int(b.sub_cx * interp), np.int(b.sub_cy * interp)),
# 7,
# (255, 255, 0)
# )
for b in mkr.desired_blob_pos:
if b is not None:
cv2.circle(f_image_interp,
(np.int(b[0] * interp), np.int(b[1] * interp)),
10, (255, 0, 0))
# draw marker border
for border_pair in itertools.combinations(mkr.marker_border, 2):
c1 = (int(border_pair[0][0] * interp),
int(border_pair[0][1]) * interp)
c2 = (int(border_pair[1][0] * interp),
int(border_pair[1][1]) * interp)
# dist = pneumodule.distance(c1, c2)
# print(dist)
if pneumodule.distance(c1,
c2) < mkr.marker_size * interp * 1.2:
cv2.line(f_image_interp, c1, c2, (255, 255, 0), 1)
# show marker region
crop_img_show = copy.deepcopy(mkr.crop_img)
if np.max(crop_img_show) > 0:
crop_img_show = crop_img_show / np.max(crop_img_show) * 255
crop_img_show = cv2.cvtColor(crop_img_show.astype(np.uint8),
cv2.COLOR_GRAY2RGB)
# display force
# for sub_r in range(1):
# for i in range(3):
# for j in range(3):
# if sub_r == 0:
# upperleft = [5 * 3 * interp, 5 * 3 * interp]
# else:
# upperleft = [5 * 3 * interp, 5 * 3 * interp]
# font = cv2.FONT_HERSHEY_SIMPLEX
# cv2.putText(
# crop_img_show,
# '%0.0f' % mkr.pressure_values[sub_r][i][j],
# (int(upperleft[0] + j * interp * 5), int(upperleft[0] + i * interp * 5)),
# font,
# 0.3, # font size
# (0, 255, 0),
# 1,
# cv2.LINE_AA
# )
# send osc messages
# center_x = []
# center_y = []
# for i in centers:
# center_x.append(i[0])
# center_y.append(i[1])
pos_x = b'/marker/%s/pos_x' % bytes(str.encode(str(marker_num)))
pos_y = b'/marker/%s/pos_y' % bytes(str.encode(str(marker_num)))
angle = b'/marker/%s/angle' % bytes(str.encode(str(marker_num)))
osc.send_message(pos_x, [mkr.pos_x])
osc_sub.send_message(pos_x, [mkr.pos_x])
osc.send_message(pos_y, [mkr.pos_y])
osc_sub.send_message(pos_y, [mkr.pos_y])
osc.send_message(angle, [mkr.rot_rad])
osc_sub.send_message(angle, [mkr.rot_rad])
for sub_r in range(5):
id_list = [0] * 4
for i in range(3):
for j in range(3):
msg = b'/marker/%s/force/%s/%s' % (
bytes(str.encode(str(marker_num))),
bytes(str.encode(str(sub_r))),
bytes(str.encode(str(i * 3 + j))))
osc.send_message(msg,
[mkr.pressure_values[sub_r][i][j]])
osc_sub.send_message(
msg, [mkr.pressure_values[sub_r][i][j]])
# determine id
if i == 0 and j == 0:
if mkr.pressure_values[sub_r][i][j] > 100:
id_list[0] = 1
if i == 0 and j == 2:
if mkr.pressure_values[sub_r][i][j] > 100:
id_list[1] = 1
if i == 2 and j == 0:
if mkr.pressure_values[sub_r][i][j] > 100:
id_list[2] = 1
if i == 2 and j == 2:
if mkr.pressure_values[sub_r][i][j] > 100:
id_list[3] = 1
# print(id_list)
msg = b'/marker/%s/id/%s' % (bytes(str.encode(
str(marker_num))), bytes(str.encode(str(sub_r))))
if id_list == [1, 0, 0, 0]:
osc.send_message(msg, [1])
osc_sub.send_message(msg, [1])
elif id_list == [0, 1, 0, 0]:
osc.send_message(msg, [2])
osc_sub.send_message(msg, [2])
elif id_list == [0, 1, 0, 0]:
osc.send_message(msg, [2])
osc_sub.send_message(msg, [2])
elif id_list == [0, 0, 1, 0]:
osc.send_message(msg, [3])
osc_sub.send_message(msg, [3])
elif id_list == [0, 1, 1, 0]:
osc.send_message(msg, [4])
osc_sub.send_message(msg, [4])
elif id_list == [1, 0, 1, 0]:
osc.send_message(msg, [5])
osc_sub.send_message(msg, [5])
elif id_list == [1, 1, 0, 0]:
osc.send_message(msg, [6])
osc_sub.send_message(msg, [6])
elif id_list == [0, 0, 0, 1]:
osc.send_message(msg, [7])
osc_sub.send_message(msg, [7])
if id_list == [1, 0, 0, 1]:
osc.send_message(msg, [8])
osc_sub.send_message(msg, [8])
elif id_list == [0, 1, 0, 1]:
osc.send_message(msg, [9])
osc_sub.send_message(msg, [9])
elif id_list == [0, 1, 0, 1]:
osc.send_message(msg, [10])
osc_sub.send_message(msg, [10])
elif id_list == [0, 0, 1, 1]:
osc.send_message(msg, [11])
osc_sub.send_message(msg, [11])
elif id_list == [0, 1, 1, 1]:
osc.send_message(msg, [12])
osc_sub.send_message(msg, [12])
elif id_list == [1, 0, 1, 1]:
osc.send_message(msg, [13])
osc_sub.send_message(msg, [13])
elif id_list == [1, 1, 0, 1]:
osc.send_message(msg, [14])
osc_sub.send_message(msg, [14])
elif id_list == [0, 0, 0, 0]:
osc.send_message(msg, [0])
osc_sub.send_message(msg, [0])
marker_num += 1
img2.setImage(np.rot90(crop_img_show, 3),
autoLevels=True,
levels=(0, 80))
# print(mkr.pressure_values[0].astype(np.int))
# img2.setImage(np.rot90(mkr.sub_region, 3), autoLevels=True, levels=(0, 80))
# print(np.sum(mkr.sub_region))
# print(mkr.sub_region[0:2, 0:2])
# print(mkr.sub_region[0 * 5 * 3 - 1:(0 + 1) * 5 * 3, 0 * 5 * 3 - 1:(0 + 1) * 5 * 3])
# c1 = (int(mkr.blobs[1].sub_cx * interp), int(mkr.blobs[1].sub_cy * interp))
# c2 = (int(mkr.blobs[2].sub_cx * interp), int(mkr.blobs[2].sub_cy * interp))
# cv2.line(f_image_interp, c1, c2, (255, 255, 0), 1)
# display force
if len(blobs) > 0:
for b in blobs:
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(
f_image_interp,
'%0.0f' % b.force,
(int(b.cx * interp) + 5, int(b.cy * interp) + 10),
font,
0.5, # font size
(0, 255, 0),
1,
cv2.LINE_AA)
# # display appeared time
# if len(blobs) > 0:
# for b in blobs:
# font = cv2.FONT_HERSHEY_SIMPLEX
# cv2.putText(
# f_image_show,
# '%5.2f' % b.t_appeared,
# (int(b.cx), int(b.cy) + 10),
# font,
# 0.3, # font size
# (0, 255, 255),
# 1,
# cv2.LINE_AA
# )
# display ID
# if len(blobs) > 0:
# for b in blobs:
# font = cv2.FONT_HERSHEY_SIMPLEX
# cv2.putText(
# f_image_show,
# 'ID: %d' % b.ID,
# (int(b.cx) + 5, int(b.cy) + 0),
# font,
# 0.3, # font size
# (255, 255, 255),
# 1,
# cv2.LINE_AA
# )
# show peaks
if len(blobs) > 0:
for b in blobs:
cv2.circle(f_image_interp,
(np.int(b.sub_cx * interp), np.int(b.sub_cy * interp)),
3, (255, 0, 0))
# # display coords
# if len(blobs) > 0:
# for b in blobs:
# font = cv2.FONT_HERSHEY_SIMPLEX
# cv2.putText(
# f_image_interp,
# '(%0.2f, %0.2f)' % (b.sub_cx, b.sub_cy),
# (int(b.cx * interp) + 5, int(b.cy * interp) + 0),
# font,
# 0.5, # font size
# (255, 255, 255),
# 1,
# cv2.LINE_AA
# )
# img1.setImage(np.rot90(f_image, 3), autoLevels=True, levels=(0, 50))
# img1.setImage(np.rot90(f_image_peaks, 3), autoLevels=True, levels=(0, 50))
# adjust sensitivity
# img1.setImage(np.rot90(f_image, 3), autoLevels=False, levels=(0, 255))
# img2.setImage(np.rot90(f_image_show, 3), autoLevels=True, levels=(0, 80))
# img3.setImage(np.rot90(f_image_thre, 3), autoLevels=True, levels=(0, 80))
img1.setImage(np.rot90(f_image_interp, 3), autoLevels=True, levels=(0, 80))
# img1.setImage(np.rot90(f_image_interp, 3), autoLevels=True, levels=(0, 80))
now = time()
dt = now - lastTime
lastTime = now
if fps is None:
fps = 1.0 / dt
else:
s = np.clip(dt * 3., 0, 1)
fps = fps * (1 - s) + (1.0 / dt) * s
print('%0.2f fps' % fps)
QtGui.QApplication.processEvents()