From 7b083ccd1417064e2ebfde55a74f7f617db2bc21 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Wed, 19 Oct 2016 14:59:21 +0000 Subject: [PATCH] - added PiCameraPipeline - added Lasertrack - picam_test uses PiCameraPipeline git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@324 b431acfa-c32f-4a4a-93f1-934dc6c82436 --- PiCameraPipeline.py | 207 ++++++++++++++++++++++++++ lasertrack.py | 141 ++++++++++++++++++ piCameraPipeline/PiCameraPipeline.py | 213 +++++++++++++++++++++++++++ picam_test.py | 88 ++++++----- 4 files changed, 609 insertions(+), 40 deletions(-) create mode 100755 PiCameraPipeline.py create mode 100755 lasertrack.py create mode 100755 piCameraPipeline/PiCameraPipeline.py diff --git a/PiCameraPipeline.py b/PiCameraPipeline.py new file mode 100755 index 0000000..b6938db --- /dev/null +++ b/PiCameraPipeline.py @@ -0,0 +1,207 @@ +# import the necessary packages +from __future__ import print_function +from imutils.video import FPS +from picamera import PiCamera +from picamera import array +import imutils +import time +import numpy as np +from threading import Thread +from threading import Lock +from threading import Semaphore + +class Fifo(object): + def __init__(self, dim=(4, 320, 240, 3), dtype=np.uint8): + self.size = 0 + self.capacity = dim[0] + self.wi = 0 + self.ri = 0 + self.buffer = np.empty(dim, dtype=dtype) + self.lock = Lock() + self.sema = Semaphore(value=0) + + def write(self, data): + shouldNotify = False + + self.lock.acquire() + + self.buffer[self.wi] = data + self.wi += 1 + + if self.wi == self.capacity: + self.wi = 0 + + if self.size < self.capacity: + self.size += 1 + shouldNotify = True + + self.lock.release() + + if shouldNotify: + self.sema.release() + + def read(self, timeout=None): + data = None + + if self.sema.acquire(blocking=True, timeout=timeout): + + self.lock.acquire() + + data = self.buffer[self.ri] + self.ri += 1 + + if self.ri == self.capacity: + self.ri = 0 + + self.size -= 1 + + self.lock.release() + + return data + +class RgbProcess(Fifo): + def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None): + super(RgbProcess, self).__init__((numEntries, ) + resolution) + self.next = next + self.thread = Thread(target=self.run) + self.cancel = False + self.fps = None + self.frames = 0 + + # called on construction + def onConstruct(self): + pass + + # called on first frame + def onFirstFrame(self): + self.thread.start() + if self.next is not None: + self.next.onFirstFrame() + + # may be overriden to process in caller context + def onData(self, data): + self.write(data) + + # thread main loop + def run(self): + self.fps = FPS().start() + while not self.cancel: + self.process() + self.frames += 1 + + # abstractmethod + def process(self): + pass + + # should be called to exit thread + def stop(self): + self.fps.stop() + print("Processed " + str(self.frames) + " frames") + print("FPS = " + str(self.frames/self.fps.elapsed())) + self.cancel = True + self.thread.join() + if self.next is not None: + self.next.stop() + +class RgbProcessorAdapter(object): + def __init__(self, resolution=(320, 240, 3)): + self.resolution = resolution + self.frameSize = resolution[0]*resolution[1]*resolution[2] + self.size = 0 + self.frames = 0 + self.fps = None + self.processors = [] + + def processorAdd(self, processor): + self.processors.append(processor) + processor.onConstruct() + + def writable(self): + return True + + def write(self, data): + if self.size == 0: + for proc in self.processors: + proc.onFirstFrame() + + self.fps = FPS().start() + + self.size += len(data) + self.frames += len(data)/self.frameSize + + res = (self.resolution[0], self.resolution[1]) + + +# rgbData = array.bytes_to_rgb(data, res) + rgbData = data + for proc in self.processors: + proc.onData(rgbData) + + def size(self): + return self.size + + def flush(self): + if self.size > 0: + self.fps.stop() + print("Recorded " + str(self.frames) + " frames (" + str(self.size) + " bytes)") + print("FPS = " + str(self.frames/self.fps.elapsed())) + +if ("__main__" == __name__): + import cv2 + + # Unit test with to threaded processors + class MyRgbProcess1(RgbProcess): + def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None): + super(MyRgbProcess1, self).__init__(resolution, numEntries, next) + + def process(self): + data = self.read(1.0) + if data is not None: + gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY) + gray_blurred = cv2.medianBlur(gray,5) + + cv2.imshow('displayThread1', gray_blurred) + cv2.waitKey(1) + + if self.next is not None: + self.next.write(cv2.cvtColor(gray_blurred,cv2.COLOR_GRAY2BGR)) + + class MyRgbProcess2(RgbProcess): + def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None): + super(MyRgbProcess2, self).__init__(resolution, numEntries) + + def process(self): + data = self.read(1.0) + if data is not None: + gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY) + + # Canny edge detection + canny = cv2.Canny(gray, 100, 50) + cv2.imshow('displayThread2', canny) + cv2.waitKey(1) + + # initialize the camera and stream + camera = PiCamera() + camera.resolution = (320, 240) + camera.framerate = 60 + frameDim = (camera.resolution[0], camera.resolution[1], 3) + myProcessor2 = MyRgbProcess2((240, 320, 3), 4) + myProcessor1 = MyRgbProcess1((240, 320, 3), 4, myProcessor2) + myRgbProcessorAdapter = RgbProcessorAdapter(frameDim) + myRgbProcessorAdapter.processorAdd(myProcessor1) + camera.start_recording(myRgbProcessorAdapter, format="bgr") + + dictFifo = Fifo(dim=(8,), dtype=dict) + dictFifo.write({'a':1, 'b':2}) + print(dictFifo.read()) + + # allow the camera to warmup and start the FPS counter + print("[INFO] sampling frames from `picamera` module...") + time.sleep(10.0) + camera.stop_recording() + myProcessor1.stop() + + # do a bit of cleanup + cv2.destroyAllWindows() + camera.close() + diff --git a/lasertrack.py b/lasertrack.py new file mode 100755 index 0000000..9a15e03 --- /dev/null +++ b/lasertrack.py @@ -0,0 +1,141 @@ +# import the necessary packages +from __future__ import print_function + +from imutils.video import FPS + +import sys +import numpy as np +import argparse +import time +import cv2 +import pprint +import math +import struct +import ev3.ev3 as ev3 +from threading import Thread + +from matplotlib import pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + +from picamera import PiCamera +from piCameraPipeline import PiCameraPipeline +from piCameraPipeline.PiCameraPipeline import RgbProcess +from piCameraPipeline.PiCameraPipeline import RgbProcessorAdapter + +width = 320 +height = 240 + +# construct the argument parse and parse the arguments +ap = argparse.ArgumentParser() +ap.add_argument("-n", "--seconds", type=int, default=10, + help="# of seconds to run") +ap.add_argument("-r", "--framerate", type=int, default=30, + help="Framerate in frame/s") +ap.add_argument("-f", "--filename", type=str, default='piCamera', + help="Whether or not frames should be displayed") +args = vars(ap.parse_args()) + +videoFile = args["filename"] +framerate = args["framerate"] +seconds = args["seconds"] + +class VideoSource(RgbProcessorAdapter): + def __init__(self, fileName, resolution, framerate): + super(VideoSource, self).__init__(resolution) + self.fileName = fileName + self.resolution = resolution + self.framerate = framerate + self.thread = None + self.camera = None + self.cancel = False + if fileName == 'piCamera': + self.camera = PiCamera() + self.camera.resolution = (self.resolution[0], self.resolution[1]) + self.camera.framerate = self.framerate + else: + self.needRgbConversion = False + self.thread = Thread(target=self.fileReadThread) + + def fileReadThread(self): + pass + + def start(self): + if self.thread is not None: + self.thread.start() + pass + else: + self.camera.start_recording(self, format="bgr") + + def stop(self): + if self.thread is not None: + self.cancel = True + self.thread.join() + else: + self.camera.stop_recording() + + def fileReadThread(self): + cap = cv2.VideoCapture(self.fileName) + while not self.cancel: + ret, frame = cap.read() + self.write(frame) + time.sleep(1.0/self.framerate) + + +class MyRgbProcess(RgbProcess): + def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None): + super(MyRgbProcess, self).__init__(resolution, numEntries, next) + + def process(self): + data = self.read(1.0) + if data is not None: + gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY) +# gray_blurred = cv2.medianBlur(gray,5) + gray_blurred = cv2.GaussianBlur(gray,(5,5),0) + ret,thresh = cv2.threshold(gray_blurred,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU) + # thresh = cv2.adaptiveThreshold(gray_blurred,255,cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY,11,2) + # thresh = cv2.adaptiveThreshold(gray_blurred,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY,11,2) + + # 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) + + img1_objects = data.copy() + 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) + pass + + cv2.imshow('Contours', img1_contours) + cv2.imshow('Objects', img1_objects) + cv2.waitKey(1) + + if self.next is not None: + self.next.write(cv2.cvtColor(gray_blurred,cv2.COLOR_GRAY2BGR)) + +# Connect to brick +#with ev3.EV3() as brick: + +# created a *threaded *video stream, allow the camera sensor to warmup, +# and start the FPS counter + +videoSource = VideoSource(videoFile, (width,height,3), framerate) +proc = MyRgbProcess((240, 320, 3), 4) +videoSource.processorAdd(proc) +videoSource.start() + +print("[INFO] sampling THREADED frames from `" + videoFile + "` at " + str(framerate) + " frame/s") +time.sleep(seconds) +videoSource.stop() +proc.stop() + +# do a bit of cleanup +cv2.destroyAllWindows() + + diff --git a/piCameraPipeline/PiCameraPipeline.py b/piCameraPipeline/PiCameraPipeline.py new file mode 100755 index 0000000..d669c18 --- /dev/null +++ b/piCameraPipeline/PiCameraPipeline.py @@ -0,0 +1,213 @@ +# import the necessary packages +from __future__ import print_function +from imutils.video import FPS +from picamera import PiCamera +from picamera import array +import imutils +import time +import numpy as np +from threading import Thread +from threading import Lock +from threading import Semaphore + +class Fifo(object): + def __init__(self, dim=(4, 320, 240, 3), dtype=np.uint8): + self.size = 0 + self.capacity = dim[0] + self.wi = 0 + self.ri = 0 + self.buffer = np.empty(dim, dtype=dtype) + self.lock = Lock() + self.sema = Semaphore(value=0) + + def write(self, data): + shouldNotify = False + + self.lock.acquire() + + self.buffer[self.wi] = data + self.wi += 1 + + if self.wi == self.capacity: + self.wi = 0 + + if self.size < self.capacity: + self.size += 1 + shouldNotify = True + + self.lock.release() + + if shouldNotify: + self.sema.release() + + def read(self, timeout=None): + data = None + + if self.sema.acquire(blocking=True, timeout=timeout): + + self.lock.acquire() + + data = self.buffer[self.ri] + self.ri += 1 + + if self.ri == self.capacity: + self.ri = 0 + + self.size -= 1 + + self.lock.release() + + return data + +class RgbProcess(Fifo): + def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None): + super(RgbProcess, self).__init__((numEntries, ) + resolution) + self.next = next + self.thread = Thread(target=self.run) + self.cancel = False + self.fps = None + self.frames = 0 + + # called on construction + def onConstruct(self): + pass + + # called on first frame + def onFirstFrame(self): + self.thread.start() + if self.next is not None: + self.next.onFirstFrame() + + # may be overriden to process in caller context + def onData(self, data): + self.write(data) + + # thread main loop + def run(self): + self.fps = FPS().start() + while not self.cancel: + self.process() + self.frames += 1 + + # abstractmethod + def process(self): + pass + + # should be called to exit thread + def stop(self): + self.fps.stop() + print("Processed " + str(self.frames) + " frames") + print("FPS = " + str(self.frames/self.fps.elapsed())) + self.cancel = True + self.thread.join() + if self.next is not None: + self.next.stop() + +class RgbProcessorAdapter(object): + def __init__(self, resolution=(320, 240, 3)): + self.resolution = resolution + self.frameSize = resolution[0]*resolution[1]*resolution[2] + self.size = 0 + self.frames = 0 + self.fps = None + self.processors = [] + self.needRgbConversion = True + + def processorAdd(self, processor): + self.processors.append(processor) + processor.onConstruct() + + def writable(self): + return True + + def write(self, data): + if self.size == 0: + for proc in self.processors: + proc.onFirstFrame() + + self.fps = FPS().start() + + if data is None: + return + + self.size += len(data) + self.frames += len(data)/self.frameSize + + res = (self.resolution[0], self.resolution[1]) + + if self.needRgbConversion: + rgbData = array.bytes_to_rgb(data, res) + else: + rgbData = data + + for proc in self.processors: + proc.onData(rgbData) + + def size(self): + return self.size + + def flush(self): + if self.size > 0: + self.fps.stop() + print("Recorded " + str(self.frames) + " frames (" + str(self.size) + " bytes)") + print("FPS = " + str(self.frames/self.fps.elapsed())) + +if ("__main__" == __name__): + import cv2 + + # Unit test with to threaded processors + class MyRgbProcess1(RgbProcess): + def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None): + super(MyRgbProcess1, self).__init__(resolution, numEntries, next) + + def process(self): + data = self.read(1.0) + if data is not None: + gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY) + gray_blurred = cv2.medianBlur(gray,5) + + cv2.imshow('displayThread1', gray_blurred) + cv2.waitKey(1) + + if self.next is not None: + self.next.write(cv2.cvtColor(gray_blurred,cv2.COLOR_GRAY2BGR)) + + class MyRgbProcess2(RgbProcess): + def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None): + super(MyRgbProcess2, self).__init__(resolution, numEntries) + + def process(self): + data = self.read(1.0) + if data is not None: + gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY) + + # Canny edge detection + canny = cv2.Canny(gray, 100, 50) + cv2.imshow('displayThread2', canny) + cv2.waitKey(1) + + # initialize the camera and stream + camera = PiCamera() + camera.resolution = (320, 240) + camera.framerate = 60 + frameDim = (camera.resolution[0], camera.resolution[1], 3) + myProcessor2 = MyRgbProcess2((240, 320, 3), 4) + myProcessor1 = MyRgbProcess1((240, 320, 3), 4, myProcessor2) + myRgbProcessorAdapter = RgbProcessorAdapter(frameDim) + myRgbProcessorAdapter.processorAdd(myProcessor1) + camera.start_recording(myRgbProcessorAdapter, format="bgr") + + dictFifo = Fifo(dim=(8,), dtype=dict) + dictFifo.write({'a':1, 'b':2}) + print(dictFifo.read()) + + # allow the camera to warmup and start the FPS counter + print("[INFO] sampling frames from `picamera` module...") + time.sleep(10.0) + camera.stop_recording() + myProcessor1.stop() + + # do a bit of cleanup + cv2.destroyAllWindows() + camera.close() + diff --git a/picam_test.py b/picam_test.py index 7e0db88..0271905 100755 --- a/picam_test.py +++ b/picam_test.py @@ -1,54 +1,62 @@ # import the necessary packages -from __future__ import print_function -from imutils.video import FPS -from picamera import PiCamera -from picamera import CircularIO -import imutils import time -import cv2 +from picamera import PiCamera +from piCameraPipeline import PiCameraPipeline +from piCameraPipeline.PiCameraPipeline import RgbProcess +from piCameraPipeline.PiCameraPipeline import RgbProcessorAdapter -class MyCameraIo(object): - def __init__(self, frameSize): - print("MyCameraIo") - self.frameSize = frameSize[0]*frameSize[1] - self.size = 0 - self.frames = 0 - self.fps = None +if ("__main__" == __name__): + import cv2 - def writable(self): - return True + # Unit test with to threaded processors + class MyRgbProcess1(RgbProcess): + def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None): + super(MyRgbProcess1, self).__init__(resolution, numEntries, next) - def write(self, data): - if self.size == 0: - self.fps = FPS().start() + def process(self): + data = self.read(1.0) + if data is not None: + gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY) + gray_blurred = cv2.medianBlur(gray,5) - self.size += len(data) - self.frames += len(data)/(3*self.frameSize) + cv2.imshow('displayThread1', gray_blurred) + cv2.waitKey(1) - def size(self): - print("size") - return self.size + if self.next is not None: + self.next.write(cv2.cvtColor(gray_blurred,cv2.COLOR_GRAY2BGR)) - def flush(self): - if self.size > 0: - self.fps.stop() - print("Recorded " + str(self.frames) + " frames (" + str(self.size) + " bytes)") - print("FPS = " + str(self.frames/self.fps.elapsed())) + class MyRgbProcess2(RgbProcess): + def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None): + super(MyRgbProcess2, self).__init__(resolution, numEntries) + def process(self): + data = self.read(1.0) + if data is not None: + gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY) -# initialize the camera and stream -camera = PiCamera() -camera.resolution = (320, 240) -camera.framerate = 90 + # Canny edge detection + canny = cv2.Canny(gray, 100, 50) + cv2.imshow('displayThread2', canny) + cv2.waitKey(1) -myCameraIo = MyCameraIo(camera.resolution) -camera.start_recording(myCameraIo, format="bgr") + # initialize the camera and stream + camera = PiCamera() + camera.resolution = (320, 240) + camera.framerate = 60 + frameDim = (camera.resolution[0], camera.resolution[1], 3) + myProcessor2 = MyRgbProcess2((240, 320, 3), 4) + myProcessor1 = MyRgbProcess1((240, 320, 3), 4, myProcessor2) + myRgbProcessorAdapter = RgbProcessorAdapter(frameDim) + myRgbProcessorAdapter.processorAdd(myProcessor1) + camera.start_recording(myRgbProcessorAdapter, format="bgr") -# allow the camera to warmup and start the FPS counter -print("[INFO] sampling frames from `picamera` module...") -time.sleep(10.0) + # allow the camera to warmup and start the FPS counter + print("[INFO] sampling frames from `picamera` module...") + time.sleep(10.0) + camera.stop_recording() + myProcessor1.stop() -# do a bit of cleanup -cv2.destroyAllWindows() -camera.close() + # do a bit of cleanup + cv2.destroyAllWindows() + camera.close()