implementing direction in tracker
This commit is contained in:
parent
b10c99d25c
commit
05e64cdefa
|
|
@ -7,119 +7,138 @@ import cv2
|
|||
from picamera2 import MappedArray, Picamera2, Preview
|
||||
from picamera2.previews.qt import QGlPicamera2
|
||||
import numpy as np
|
||||
from pythonosc import udp_client
|
||||
|
||||
|
||||
def rectFromPoint(center, len, width, axis):
|
||||
rect = ((0, 0), (0, 0))
|
||||
l = int(len/2)
|
||||
w = int(width/2)
|
||||
if(axis == 'x'):
|
||||
rect = ((center[0] - l, center[1] - w), (center[0] + l, center[1] + w))
|
||||
elif(axis == 'y'):
|
||||
rect = ((center[0] - w, center[1] - l), (center[0] + w, center[1] + l))
|
||||
return rect
|
||||
rect = ((0, 0), (0, 0))
|
||||
l = int(len/2)
|
||||
w = int(width/2)
|
||||
if(axis == 'x'):
|
||||
rect = ((center[0] - l, center[1] - w), (center[0] + l, center[1] + w))
|
||||
elif(axis == 'y'):
|
||||
rect = ((center[0] - w, center[1] - l), (center[0] + w, center[1] + l))
|
||||
return rect
|
||||
|
||||
|
||||
def rectsFromPoint(center, l1, l2, l3, w, axis):
|
||||
centerFine = center
|
||||
fineInner = rectFromPoint(centerFine, l1, w, axis)
|
||||
fineOuter = rectFromPoint(centerFine, l2, w, axis)
|
||||
centerCoarse = center
|
||||
if(axis == 'x'):
|
||||
centerCoarse = (center[0], center[1] + w)
|
||||
elif(axis == 'y'):
|
||||
centerCoarse = (center[0] + w, center[1])
|
||||
coarse = rectFromPoint(centerCoarse, l3, w, axis)
|
||||
return [fineInner, fineOuter, coarse, center]
|
||||
|
||||
centerFine = center
|
||||
fineInner = rectFromPoint(centerFine, l1, w, axis)
|
||||
if(axis == 'x'):
|
||||
fineInnerNeg = rectFromPoint((centerFine[0] - int(l1 / 4), centerFine[1]), int(l1 / 2), w, axis)
|
||||
fineInnerPos = rectFromPoint((centerFine[0] + int(l1 / 4), centerFine[1]), int(l1 / 2), w, axis)
|
||||
elif(axis == 'y'):
|
||||
fineInnerNeg = rectFromPoint((centerFine[0], centerFine[1] - int(l1 / 4)), int(l1 / 2), w, axis)
|
||||
fineInnerPos = rectFromPoint((centerFine[0], centerFine[1] + int(l1 / 4)), int(l1 / 2), w, axis)
|
||||
|
||||
fineOuter = rectFromPoint(centerFine, l2, w, axis)
|
||||
|
||||
centerCoarse = center
|
||||
if(axis == 'x'):
|
||||
centerCoarse = (center[0], center[1] + w)
|
||||
elif(axis == 'y'):
|
||||
centerCoarse = (center[0] + w, center[1])
|
||||
coarse = rectFromPoint(centerCoarse, l3, w, axis)
|
||||
|
||||
return [fineInnerNeg, fineInnerPos, fineInner, fineOuter, coarse, center]
|
||||
|
||||
|
||||
def moveROI(event, x, y, flags, params):
|
||||
global roiX, roiY, moving, l1, l2, l3, w, selectedAxis
|
||||
if event == cv2.EVENT_LBUTTONDOWN:
|
||||
moving = True
|
||||
global roiX, roiY, moving, l1, l2, l3, w, selectedAxis
|
||||
if event == cv2.EVENT_LBUTTONDOWN:
|
||||
moving = True
|
||||
|
||||
elif event==cv2.EVENT_MOUSEMOVE:
|
||||
if moving==True:
|
||||
if(selectedAxis == 'x'):
|
||||
roiX = rectsFromPoint((x, y), l1, l2, l3, w, selectedAxis)
|
||||
elif(selectedAxis == 'y'):
|
||||
roiY = rectsFromPoint((x, y), l1, l2, l3, w, selectedAxis)
|
||||
elif event==cv2.EVENT_MOUSEMOVE:
|
||||
if moving==True:
|
||||
if(selectedAxis == 'x'):
|
||||
roiX = rectsFromPoint((x, y), l1, l2, l3, w, selectedAxis)
|
||||
elif(selectedAxis == 'y'):
|
||||
roiY = rectsFromPoint((x, y), l1, l2, l3, w, selectedAxis)
|
||||
|
||||
elif event == cv2.EVENT_LBUTTONUP:
|
||||
moving = False
|
||||
elif event == cv2.EVENT_LBUTTONUP:
|
||||
moving = False
|
||||
|
||||
|
||||
def crop(frame, rect):
|
||||
return frame[rect[0][1]:rect[1][1], rect[0][0]:rect[1][0]]
|
||||
return frame[rect[0][1]:rect[1][1], rect[0][0]:rect[1][0]]
|
||||
|
||||
|
||||
def replaceCrop(frame, rect, crop):
|
||||
frame[rect[0][1]:rect[1][1], rect[0][0]:rect[1][0]] = crop
|
||||
frame[rect[0][1]:rect[1][1], rect[0][0]:rect[1][0]] = crop
|
||||
|
||||
|
||||
def genDKernel(dVal):
|
||||
return np.ones((dVal, dVal), np.uint8)
|
||||
return np.ones((dVal, dVal), np.uint8)
|
||||
|
||||
|
||||
def track(frame, roi, dKernel):
|
||||
exp = 2
|
||||
roiFineInner, roiFineOuter, roiCourse, roiCenter = roi
|
||||
exp = 2
|
||||
roiFineInnerNeg, roiFineInnerPos, roiFineInner, roiFineOuter, roiCourse, roiCenter = roi
|
||||
|
||||
cropFineOuter = crop(frame, roiFineOuter)
|
||||
cropCoarse = crop(frame, roiCourse)
|
||||
#gray = cv2.cvtColor(crop, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
dilation = cv2.dilate(cropFineOuter, dKernel, iterations=1)
|
||||
ret,thresh = cv2.threshold(dilation,100,255,cv2.THRESH_BINARY)
|
||||
|
||||
cropFineOuter = crop(frame, roiFineOuter)
|
||||
cropCoarse = crop(frame, roiCourse)
|
||||
#gray = cv2.cvtColor(crop, cv2.COLOR_BGR2GRAY)
|
||||
replaceCrop(frame, roiFineOuter, thresh)
|
||||
|
||||
meanCourse = pow(cropCoarse.mean(), 1)
|
||||
mean = 0
|
||||
direction = 1
|
||||
if(meanCourse > 10):
|
||||
# this could potentially be made more efficient by cropping from cropFineOuter
|
||||
cropFineInner = crop(frame, roiFineInner)
|
||||
# this could potentially be made more efficient by cropping from cropFineInner
|
||||
cropFineInnerNeg = crop(frame, roiFineInnerNeg)
|
||||
cropFineInnerPos = crop(frame, roiFineInnerPos)
|
||||
|
||||
dilation = cv2.dilate(cropFineOuter, dKernel, iterations=1)
|
||||
ret,thresh = cv2.threshold(dilation,100,255,cv2.THRESH_BINARY)
|
||||
|
||||
replaceCrop(frame, roiFineOuter, thresh)
|
||||
|
||||
# this could potentially be made more efficient by cropping from cropFineOuter
|
||||
cropFineInner = crop(frame, roiFineInner)
|
||||
|
||||
meanFine = pow(cropFineInner.mean(), exp)
|
||||
meanCourse = pow(cropCoarse.mean(), 1)
|
||||
mean = 0
|
||||
if(meanCourse > 10):
|
||||
mean = meanFine
|
||||
distance = pow(255, exp) - mean
|
||||
|
||||
return distance
|
||||
meanFine = pow(cropFineInner.mean(), exp)
|
||||
direction = np.sign(cropFineInnerPos.mean() - cropFineInnerNeg.mean())
|
||||
|
||||
mean = meanFine
|
||||
|
||||
distance = direction * (pow(255, exp) - mean)
|
||||
|
||||
return distance
|
||||
|
||||
|
||||
def drawRect(frame, points):
|
||||
cv2.rectangle(frame, points[0], points[1], (0, 255, 0))
|
||||
cv2.rectangle(frame, points[0], points[1], (0, 255, 0))
|
||||
|
||||
|
||||
def drawRoi(frame, roi):
|
||||
for rect in roi[:3]:
|
||||
drawRect(frame, rect)
|
||||
center = roi[3]
|
||||
cv2.line(frame, (center[0] - 5, center[1]), (center[0] + 5, center[1]), (0, 255, 0), 1)
|
||||
cv2.line(frame, (center[0], center[1] - 5), (center[0], center[1] + 5), (0, 255, 0), 1)
|
||||
for rect in roi[2:5]:
|
||||
drawRect(frame, rect)
|
||||
center = roi[5]
|
||||
cv2.line(frame, (center[0] - 5, center[1]), (center[0] + 5, center[1]), (0, 255, 0), 1)
|
||||
cv2.line(frame, (center[0], center[1] - 5), (center[0], center[1] + 5), (0, 255, 0), 1)
|
||||
|
||||
def picameraToCVTrack():
|
||||
global roiX, roiY, moving, l1, l2, l3, w, selectedAxis, dilationKernel, calibrate
|
||||
global roiX, roiY, moving, l1, l2, l3, w, selectedAxis, dilationKernel, calibrate, oscClient
|
||||
|
||||
while True:
|
||||
frame = picam2.capture_buffer("lores")
|
||||
frame = frame[:s1 * h1].reshape((h1, s1))
|
||||
#frame = picam2.capture_array("lores")
|
||||
#frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2RGB)
|
||||
|
||||
|
||||
distanceX = track(frame, roiX, dilationKernel)
|
||||
distanceY = track(frame, roiY, dilationKernel)
|
||||
|
||||
oscClient.send_message("/distanceX", distanceX)
|
||||
|
||||
drawRoi(frame, roiX)
|
||||
drawRoi(frame, roiY)
|
||||
|
||||
cv2.putText(frame, "{}: {:.2f}".format("distance x", distanceX), (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
|
||||
cv2.putText(frame, "{}: {:.2f}".format("distance y", distanceY), (10, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 3)
|
||||
|
||||
|
||||
if calibrate:
|
||||
cv2.imshow("Frame", frame)
|
||||
#cv2.imshow("Process", tresh)
|
||||
|
||||
|
||||
# Press Q on keyboard to exit
|
||||
key = cv2.waitKey(20)
|
||||
#if key == 32:
|
||||
|
|
@ -140,22 +159,21 @@ def picameraToCVTrack():
|
|||
calibrate = False
|
||||
cv2.destroyAllWindows()
|
||||
else:
|
||||
print("hello")
|
||||
calibrate = True
|
||||
cv2.startWindowThread()
|
||||
#elif key == ord('q'):
|
||||
# break
|
||||
|
||||
|
||||
|
||||
class TrackerThread(QThread):
|
||||
def __init__(self, target=None):
|
||||
super().__init__()
|
||||
self.target = target
|
||||
|
||||
def run(self):
|
||||
if self.target:
|
||||
self.target()
|
||||
|
||||
def __init__(self, target=None):
|
||||
super().__init__()
|
||||
self.target = target
|
||||
|
||||
def run(self):
|
||||
if self.target:
|
||||
self.target()
|
||||
|
||||
|
||||
class MainWindow(QGlPicamera2):
|
||||
def __init__(self, *args, **kwargs):
|
||||
|
|
@ -166,15 +184,15 @@ class MainWindow(QGlPicamera2):
|
|||
self.shortcut_close_window.activated.connect(self.goFullscreen)
|
||||
self.setWindowFlags(Qt.FramelessWindowHint)
|
||||
self.move(0, 0)
|
||||
|
||||
|
||||
def mousePressEvent(self, event):
|
||||
self.oldPos = event.globalPos()
|
||||
|
||||
|
||||
def mouseMoveEvent(self, event):
|
||||
delta = QPoint (event.globalPos() - self.oldPos)
|
||||
self.move(self.x() + delta.x(), self.y() + delta.y())
|
||||
self.oldPos = event.globalPos()
|
||||
|
||||
|
||||
def goFullscreen(self):
|
||||
if self.isFullScreen():
|
||||
#self.setWindowFlags(self._flags)
|
||||
|
|
@ -189,7 +207,7 @@ picam2 = Picamera2()
|
|||
#max resolution is (4056, 3040) which is more like 10 fps
|
||||
config = picam2.create_preview_configuration(main={"size": (2028, 1520)}, lores={"size": (1920, 1440), "format": "YUV420"})
|
||||
picam2.configure(config)
|
||||
|
||||
|
||||
app = QApplication([])
|
||||
qpicamera2 = MainWindow(picam2, width=1920, height=1440, keep_ar=False)
|
||||
qpicamera2.setWindowTitle("Qt Picamera2 App")
|
||||
|
|
@ -208,6 +226,8 @@ dilationVal = 75
|
|||
dilationKernel = genDKernel(dilationVal)
|
||||
calibrate = True
|
||||
|
||||
oscClient = udp_client.SimpleUDPClient("127.0.0.1", 57120)
|
||||
|
||||
cv2.startWindowThread()
|
||||
cv2.namedWindow("Frame")
|
||||
cv2.setMouseCallback("Frame", moveROI)
|
||||
|
|
|
|||
Loading…
Reference in a new issue