diff --git a/python/vernier_tracker.py b/python/vernier_tracker.py index a715c34..45fe354 100644 --- a/python/vernier_tracker.py +++ b/python/vernier_tracker.py @@ -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 - - 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) - - 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 + 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) - return distance + 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) + + 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)