- 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

@@ -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()
+

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()
+
+

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()
+

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):
+if ("__main__" == __name__):
-	def __init__(self, frameSize):
+	import cv2
-		print("MyCameraIo")
-		self.frameSize = frameSize[0]*frameSize[1]
-		self.size = 0
-		self.frames = 0
-		self.fps = None
 
-	def writable(self):
+	# Unit test with to threaded processors
-		return True
+	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):
+		def process(self):
-		if self.size == 0:
+			data = self.read(1.0)
-			self.fps = FPS().start()
+			if data is not None:
+				gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY)
+				gray_blurred = cv2.medianBlur(gray,5)
 
-		self.size += len(data)
+				cv2.imshow('displayThread1', gray_blurred)
-		self.frames += len(data)/(3*self.frameSize)
+				cv2.waitKey(1)
 
-	def size(self):
+				if self.next is not None:
-		print("size")
+					self.next.write(cv2.cvtColor(gray_blurred,cv2.COLOR_GRAY2BGR))
-		return self.size
 
-	def flush(self):
+	class MyRgbProcess2(RgbProcess):
-		if self.size > 0:
+		def __init__(self, resolution=(320, 240, 3), numEntries=2, next=None):
-			self.fps.stop()
+			super(MyRgbProcess2, self).__init__(resolution, numEntries)
-			print("Recorded " + str(self.frames) + " frames (" + str(self.size) + " bytes)")
-			print("FPS = " + str(self.frames/self.fps.elapsed()))
 
+		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
+				# Canny edge detection
-camera = PiCamera()
+				canny = cv2.Canny(gray, 100, 50)
-camera.resolution = (320, 240)
+				cv2.imshow('displayThread2', canny)
-camera.framerate = 90
+				cv2.waitKey(1)
 
-myCameraIo = MyCameraIo(camera.resolution)
+	# initialize the camera and stream
-camera.start_recording(myCameraIo, format="bgr")
+	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
+	# allow the camera to warmup and start the FPS counter
-print("[INFO] sampling frames from `picamera` module...")
+	print("[INFO] sampling frames from `picamera` module...")
-time.sleep(10.0)
+	time.sleep(10.0)
+	camera.stop_recording()
+	myProcessor1.stop()
 
-# do a bit of cleanup
+	# do a bit of cleanup
-cv2.destroyAllWindows()
+	cv2.destroyAllWindows()
-camera.close()
+	camera.close()