diff --git a/lasertrack.py b/lasertrack.py index 7f7de1b..b785269 100755 --- a/lasertrack.py +++ b/lasertrack.py @@ -23,9 +23,6 @@ from piCameraPipeline.PiCameraPipeline import RgbProcess from piCameraPipeline.PiCameraPipeline import RgbProcessorAdapter from piCameraPipeline.VideoSource import VideoSource -width = 320 -height = 240 - # construct the argument parse and parse the arguments ap = argparse.ArgumentParser() ap.add_argument("-n", "--seconds", type=int, default=10, @@ -36,19 +33,26 @@ ap.add_argument("-e", "--with-ev3", type=str, default='off', help="Framerate in frame/s") ap.add_argument("-f", "--filename", type=str, default='piCamera', help="Choose video source") +ap.add_argument("-x", "--width", type=int, default=320, + help="width") +ap.add_argument("-y", "--height", type=int, default=240, + help="height") args = vars(ap.parse_args()) videoFile = args["filename"] framerate = args["framerate"] seconds = args["seconds"] with_ev3 = args["with_ev3"] == 'on' +width = args["width"] +height = args["height"] class MyRgbProcess(RgbProcess): def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None): super(MyRgbProcess, self).__init__(name, resolution, numEntries, next) self.brick = brick self.isVisible = False - + self.frameSkip = 3 + self.frameSkipCounter = 0 def onFirstFrame(self): self.sendBrick(0, 0) @@ -61,48 +65,52 @@ class MyRgbProcess(RgbProcess): gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY) img1_objects = data.copy() - if 1: - # Min / max approach - gray_blurred = cv2.medianBlur(gray,5) - minVal, maxVal, minLoc, maxLoc = cv2.minMaxLoc(gray_blurred) - x = maxLoc[0] - y = maxLoc[1] - if maxVal > 100*minVal: - if not self.isVisible: - print ("Visible") - self.isVisible = True - img1_objects = cv2.rectangle(img1_objects,(x-5,y-5),(x+5,y+5),(0,0,255),2) - errorX = float(x)-160 - errorY = float(y)-120 - self.sendBrick(errorX/2, errorY/2) -# print(maxLoc) - else: - if self.isVisible: - print ("NOT Visible") - self.sendBrick(0, 0) - self.isVisible = False + # Min / max approach + gray_blurred = cv2.medianBlur(gray,3) + minVal, maxVal, minLoc, maxLoc = cv2.minMaxLoc(gray_blurred) + x = maxLoc[0] + y = maxLoc[1] + rect = None + state = 0 + ref_color = 1 + for roi_size in range(1,15): + roi = gray[y-roi_size:y+roi_size, x-roi_size:x+roi_size] + mean_color = cv2.mean(roi) + if state == 0: + ref_color = mean_color[0] + if ref_color < 128: + break + state = 1 + elif state == 1: + km = mean_color[0]/ref_color + if km < 0.7: + rect = [(x-roi_size, y-roi_size),(x+roi_size, y+roi_size)] + break + + if rect is not None: + if not self.isVisible: + print ("Visible") + self.isVisible = True + img1_objects = cv2.rectangle(img1_objects,rect[0],rect[1],(255,0,0),1) + errorX = float(x)-self.res[0]/2 + errorY = float(y)-self.res[1]/2 + self.sendBrick(errorX/2, errorY/2) else: - # Contour approach - gray_blurred = cv2.GaussianBlur(gray,(5,5),0) - ret,thresh = cv2.threshold(gray_blurred,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU) - # Contours - (_, contours, hierachy) = cv2.findContours(thresh.copy(),cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) - img1_contours = np.zeros((height,width,3), np.uint8) - img1_contours = cv2.drawContours(img1_contours, contours, -1, (0,255,0), 1) - for contour in contours: - x,y,w,h = cv2.boundingRect(contour) - roi = gray[y:y+h, x:x+w] - mean_color = cv2.mean(gray) - mean_color_roi = cv2.mean(roi) - if mean_color_roi[0] > 2*mean_color[0]: - print (mean_color) - print (mean_color_roi) - img1_objects = cv2.rectangle(img1_objects,(x,y),(x+w,y+h),(0,0,255),2) + if self.isVisible: + print ("NOT Visible") + self.sendBrick(0, 0) + self.isVisible = False + + showFrame = False + if self.frameSkipCounter > 0: + self.frameSkipCounter -= 1 + else: + self.frameSkipCounter = self.frameSkip-1 + showFrame = True - cv2.imshow('Contours', img1_contours) - - cv2.imshow('Objects', img1_objects) - cv2.waitKey(1) + if showFrame: + cv2.imshow('Objects', img1_objects) + cv2.waitKey(1) if self.next is not None: self.next.write(cv2.cvtColor(gray_blurred,cv2.COLOR_GRAY2BGR)) @@ -119,7 +127,7 @@ if with_ev3: with ev3.EV3() as brick: videoSource = VideoSource(videoFile, (width,height,3), framerate) - proc = MyRgbProcess('MyRgbProcess', (240, 320, 3), 4, brick=brick) + proc = MyRgbProcess('MyRgbProcess', (height, width, 3), 4, brick=brick) videoSource.processorAdd(proc) videoSource.start() @@ -129,7 +137,7 @@ if with_ev3: proc.stop() else: videoSource = VideoSource(videoFile, (width,height,3), framerate) - proc = MyRgbProcess('MyRgbProcess', (240, 320, 3), 4) + proc = MyRgbProcess('MyRgbProcess', (height, width, 3), 4) videoSource.processorAdd(proc) videoSource.start() diff --git a/piCameraPipeline/PiCameraPipeline.py b/piCameraPipeline/PiCameraPipeline.py index 3124c20..4c0dde5 100755 --- a/piCameraPipeline/PiCameraPipeline.py +++ b/piCameraPipeline/PiCameraPipeline.py @@ -63,6 +63,7 @@ class RgbProcess(Fifo): def __init__(self, name='RgbProcess', resolution=(320, 240, 3), numEntries=2, next=None): super(RgbProcess, self).__init__((numEntries, ) + resolution) self.name = name + self.res = resolution self.next = next self.thread = Thread(target=self.run) self.cancel = False diff --git a/piCameraPipeline/VideoSource.py b/piCameraPipeline/VideoSource.py index f82f338..16d56ab 100755 --- a/piCameraPipeline/VideoSource.py +++ b/piCameraPipeline/VideoSource.py @@ -1,5 +1,6 @@ import time import cv2 +import imutils from threading import Thread from picamera import PiCamera from piCameraPipeline.PiCameraPipeline import RgbProcessorAdapter @@ -41,10 +42,19 @@ class VideoSource(RgbProcessorAdapter): def fileReadThread(self): cap = cv2.VideoCapture(self.fileName) + cap_w = cap.get(cv2.CAP_PROP_FRAME_WIDTH) + cap_h = cap.get(cv2.CAP_PROP_FRAME_HEIGHT) + needResize = False + if self.resolution[0] != cap_w or self.resolution[1] != cap_h: + needResize = True + while not self.cancel: ret, frame = cap.read() if ret: - self.write(frame) + if needResize: + self.write(imutils.resize(frame, width=self.resolution[0], height=self.resolution[1])) + else: + self.write(frame) else: break time.sleep(1.0/self.framerate)