From 944baa2cd81e79a4fc0c37216095ddd0ad6b352a Mon Sep 17 00:00:00 2001 From: Derek Campbell Date: Sat, 28 Feb 2015 15:22:22 +0000 Subject: [PATCH] Add new multithreaded tracking demo. Update existing single threaded tracking demo to maximise performance and minimise latency. --- src/gz_piter/MagPi/symbolFinder.py | 121 +++++++++++++++++++++ src/gz_piter/MagPi/tracking.py | 114 ++++++++++++++++---- src/gz_piter/MagPi/trackingWithThreads.py | 126 ++++++++++++++++++++++ 3 files changed, 338 insertions(+), 23 deletions(-) create mode 100644 src/gz_piter/MagPi/symbolFinder.py create mode 100644 src/gz_piter/MagPi/trackingWithThreads.py diff --git a/src/gz_piter/MagPi/symbolFinder.py b/src/gz_piter/MagPi/symbolFinder.py new file mode 100644 index 0000000..7105085 --- /dev/null +++ b/src/gz_piter/MagPi/symbolFinder.py @@ -0,0 +1,121 @@ +#!/usr/bin/python +# +# symbolFinder.py +# +# Author: Derek Campbell +# Date : 22/10/2014 +# +# Copyright 2014 +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. +# +# This class uses OpenCV to detect symbols of the specified colour in +# the image captured from the Raspberry Pi camera. +# +# The position of the detected symbol in the image is available via the +# 'getPatch()' method. + +import cv2 +import threading +import time +import os +import numpy as np + +class SymbolFinder(threading.Thread): + + def __init__(self): + super(SymbolFinder, self).__init__() + self.cap = cv2.VideoCapture() + self.active = True + self.dataReady = False + self.patch = None + self.gate = threading.RLock() + self.gate.acquire() + self.spotFilter = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)) + self.maskMorph = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (10, 10)) + self.low = (20, 120, 40) + self.high = (89, 255, 160) + + def run(self): + self.throttle = 5 + self.gate.acquire() + while (self.active == True): + acqTime = 0 + while acqTime < 0.01: + now = time.time() + ret, self.frame = self.cap.read() + acqTime = time.time() - now + now = time.time() + imgHSV = cv2.cvtColor(self.frame, cv2.COLOR_BGR2HSV) + cvtTime = time.time() - now + now = time.time() + mask = cv2.inRange(imgHSV, self.low, self.high) + inRangeTime = time.time() - now + now = time.time() + mask = cv2.erode(mask, self.spotFilter) # Remove spots in image + mask = cv2.dilate(mask, self.maskMorph) # Merge holes in image + maskTime = time.time() - now + # Find the contours in the mask + now = time.time() + contours, hierarchy = cv2.findContours(mask, 1, 2) + contourTime = time.time() - now + # Find the contour with the greatest area + now = time.time() + area = 0.0 + contour = None + for candidateContour in contours: + candidateArea = cv2.contourArea(candidateContour) + if candidateArea > area: + area = candidateArea + contour = candidateContour + # Save the bounding rectangle for the contour + if len(contours) > 0: + self.patch = cv2.boundingRect(contour) + rectTime = time.time() - now + self.symbolTimes = (acqTime, cvtTime, inRangeTime, maskTime, contourTime, rectTime) + self.dataReady = True + self.gate.release() + time.sleep(0.01) + self.gate.acquire() + + def enable(self): + #os.system("v4l2-ctl --set-fmt-video=width=320,height=240,pixelformat=10") + self.cap.open(0) + if (self.cap.isOpened()): + self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 320) + self.cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, 240) + #self.cap.set(cv2.cv.CV_CAP_PROP_FPS, 4) + discardFrameCount = 0 + while discardFrameCount < 25: + self.cap.read() + discardFrameCount = discardFrameCount + 1 + self.gate.release() + else: + print("ERROR: symbolFinder.py 99 : Failed to open camera") + self.active = False + + def disable(self): + self.gate.acquire() + self.cap.release() + + def stop(self): + self.disable() + self.active = False + self.gate.release() + + def getPatch(self): + self.dataReady = False + return self.patch, self.frame, self.symbolTimes diff --git a/src/gz_piter/MagPi/tracking.py b/src/gz_piter/MagPi/tracking.py index ae3eb7a..4255240 100644 --- a/src/gz_piter/MagPi/tracking.py +++ b/src/gz_piter/MagPi/tracking.py @@ -1,8 +1,35 @@ import cv2 import sys, getopt +import os import numpy as np - -obj = cv2.imread('./symbols/home_85x120w.png') +import time + +accumulator = [0.0 for j in range(20)] +count = 0.0 +def avg(values): + global count + avgs = [0.0 for j in range(0, len(values))] + count = count + 1 + for x in range(0, len(values)): + accumulator[x] = accumulator[x] + values[x] + avgs[x] = accumulator[x] / count + return tuple(avgs) + +maximums = [0.0 for j in range(20)] +def maximum(values): + for x in range(0, len(values)): + if maximums[x] < values[x]: + maximums[x] = values[x] + return tuple(maximums[0:len(values)]) + +minimums = [10000.0 for j in range(20)] +def minimum(values): + for x in range(0, len(values)): + if minimums[x] > values[x]: + minimums[x] = values[x] + return tuple(minimums[0:len(values)]) + +obj = cv2.imread('./symbols/turn_right_85x120w.png') detector = cv2.SURF(1000) kp_object, des_object = detector.detectAndCompute(obj, None) @@ -12,6 +39,9 @@ search_params = dict(checks = 50) matcher = cv2.FlannBasedMatcher(index_params, search_params) +#os.system("v4l2-ctl --set-fmt-video=width=320,height=240,pixelformat=10") +#os.system("v4l2-ctl -p 3") + cap = cv2.VideoCapture(-1) if(not cap.isOpened()): @@ -19,40 +49,53 @@ else: cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 320) cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, 240) + #cap.set(cv2.cv.CV_CAP_PROP_FPS, 3) spotFilter = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)) maskMorph = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (10, 10)) lowH = 20 - highH = 69 + highH = 89 lowS = 120 highS = 255 - lowV = 50 - highV = 185 + lowV = 40 + highV = 160 + now=time.time() + start = now while(1): frameCount = 0 - while frameCount < 5: - cap.read() - frameCount = frameCount + 1 - success, frame = cap.read() + acqTime = 0 + while acqTime < 0.01: + now = time.time() + success, frame = cap.read() + acqTime = time.time() - now if (not success): print("Cannot read a frame from the camera") else: + now = time.time() + cycleStart = now imgHSV = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - + cvtTime = time.time() - now + now=time.time() mask = cv2.inRange(imgHSV, np.array([lowH, lowS, lowV]), np.array([highH, highS, highV])) - + inRangeTime = time.time() - now + # Remove spots in image + now=time.time() mask = cv2.erode(mask, spotFilter) # Create mask mask = cv2.dilate(mask, maskMorph) + maskTime = time.time() - now # Find the contours in the mask + now=time.time() contours, hierarchy = cv2.findContours(mask, 1, 2) - + contourTime = time.time() - now + + now=time.time() # Find the contour with the greatest area area = 0.0 contour = None @@ -70,33 +113,46 @@ # Double size of rectangle x = x-(w/2) y = y-(h/2) - w = w*2 - h = h*2 - - # Get grayscale image by taking red channel. - # Its faster and green will appear black as a bonus - image = cv2.equalizeHist(cv2.split(frame)[2]) - + w = w * 2 + h = h * 2 + + if x < 0: + x = 0 + if y < 0: + y = 0 + rectTime = time.time() - now # Crop the frame to the rectangle found from the mask - image = image[y:y+h, x:x+w] - if image.size <> 0: + now = time.time() + cpy = frame[y:y+h, x:x+w] + cropTime = time.time() - now + histTime = detectTime = matchTime = dispTime = decorateTime = 0 + if cpy.size <> 0: + # Get grayscale image by taking red channel. + # Its faster and green will appear black as a bonus + now=time.time() + image = cv2.equalizeHist(cv2.split(cpy)[2]) + histTime = time.time() - now + now=time.time() kp_image, des_image = detector.detectAndCompute(image, None) - + detectTime = time.time() - now if not isinstance(des_image, type(None)): # Prevent knnMatch defect when the image # descriptor list contains only one point if len(des_image) > 1: + now=time.time() matches = matcher.knnMatch(des_object, des_image, 2) good_matches = [] for match in matches: if len(match) == 2 and match[0].distance < match[1].distance * 0.7: good_matches.append(match) - + matchTime = time.time() - now + if len(good_matches) > 4: src_pts = np.float32([ kp_object[m[0].queryIdx].pt for m in good_matches ]) dst_pts = np.float32([ kp_image[m[0].trainIdx].pt for m in good_matches ]) + now=time.time() M, mask2 = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC) obj_h,obj_w,obj_dep = obj.shape @@ -106,8 +162,20 @@ dst = dst + offset #frame = cv2.bitwise_and(frame, frame, mask = mask) cv2.polylines(frame, [np.int32(dst)], True, 255, 3) + decorateTime = time.time() - now cv2.rectangle(frame, (x,y), (x+w, y+h), (0,255,0), 2) + now=time.time() cv2.imshow("Camera View", frame) + dispTime = time.time() - now + now = time.time() + times = (acqTime, cvtTime, inRangeTime, maskTime, contourTime, rectTime, cropTime, histTime, detectTime, matchTime, decorateTime, dispTime, now - cycleStart, now - start) + avgs = avg(times) + print "AVG -> AQU: %f, CVT: %f, IRG: %f, MSK: %f, CTR: %f, RCT: %f, CRP: %f, HST: %f, DCT: %f, MCH %f, DEC: %f, DSP: %f, TTL: %f, SPF: %f" % avgs + maxs = maximum(times) + print "MAX -> AQU: %f, CVT: %f, IRG: %f, MSK: %f, CTR: %f, RCT: %f, CRP: %f, HST: %f, DCT: %f, MCH %f, DEC: %f, DSP: %f, TTL: %f, SPF: %f" % maxs + mins = minimum(times) + print "MIN -> AQU: %f, CVT: %f, IRG: %f, MSK: %f, CTR: %f, RCT: %f, CRP: %f, HST: %f, DCT: %f, MCH %f, DEC: %f, DSP: %f, TTL: %f, SPF: %f" % mins + start = now if(cv2.waitKey(1) == 27): break cap.release() diff --git a/src/gz_piter/MagPi/trackingWithThreads.py b/src/gz_piter/MagPi/trackingWithThreads.py new file mode 100644 index 0000000..47c0137 --- /dev/null +++ b/src/gz_piter/MagPi/trackingWithThreads.py @@ -0,0 +1,126 @@ +import cv2 +import sys, getopt +import numpy as np +import time +import symbolFinder + +accumulator = [0.0 for j in range(20)] +count = 0.0 +def avg(values): + global count + avgs = [0.0 for j in range(0, len(values))] + count = count + 1 + for x in range(0, len(values)): + accumulator[x] = accumulator[x] + values[x] + avgs[x] = accumulator[x] / count + return tuple(avgs) + +maximums = [0.0 for j in range(20)] +def maximum(values): + for x in range(0, len(values)): + if maximums[x] < values[x]: + maximums[x] = values[x] + return tuple(maximums[0:len(values)]) + +minimums = [10000.0 for j in range(20)] +def minimum(values): + for x in range(0, len(values)): + if minimums[x] > values[x]: + minimums[x] = values[x] + return tuple(minimums[0:len(values)]) + +obj = cv2.imread('./symbols/turn_right_85x120w.png') + +detector = cv2.SURF(1000) +kp_object, des_object = detector.detectAndCompute(obj, None) + +FLANN_INDEX_KDTREE = 0 +index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5) +search_params = dict(checks = 50) +matcher = cv2.FlannBasedMatcher(index_params, search_params) + +symFinder = symbolFinder.SymbolFinder() +symFinder.start() +symFinder.enable() + +if(not symFinder.active): + print("Cannot start Symbol Finder") +else: + now=time.time() + start = now + while(1): + if symFinder.dataReady == True: + now = time.time() + cycleTime = now + patch, frame, symTimes = symFinder.getPatch() + x, y, w, h = patch + # Double size of rectangle + x = x-(w/2) + y = y-(h/2) + w = w * 2 + h = h * 2 + + if x < 0: + x = 0 + if y < 0: + y = 0 + # Crop the frame to the rectangle found from the mask + now = time.time() + cpy = frame[y:y+h, x:x+w] + cropTime = time.time() - now + histTime = detectTime = matchTime = dispTime = decorateTime = 0 + if cpy.size <> 0: + # Get grayscale image by taking red channel. + # Its faster and green will appear black as a bonus + now=time.time() + image = cv2.equalizeHist(cv2.split(cpy)[2]) + histTime = time.time() - now + now=time.time() + kp_image, des_image = detector.detectAndCompute(image, None) + detectTime = time.time() - now + if not isinstance(des_image, type(None)): + # Prevent knnMatch defect when the image + # descriptor list contains only one point + if len(des_image) > 1: + now=time.time() + matches = matcher.knnMatch(des_object, des_image, 2) + + good_matches = [] + for match in matches: + if len(match) == 2 and match[0].distance < match[1].distance * 0.7: + good_matches.append(match) + matchTime = time.time() - now + + if len(good_matches) > 4: + src_pts = np.float32([ kp_object[m[0].queryIdx].pt for m in good_matches ]) + dst_pts = np.float32([ kp_image[m[0].trainIdx].pt for m in good_matches ]) + + now=time.time() + M, mask2 = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC) + + obj_h,obj_w,obj_dep = obj.shape + pts = np.float32([ [0,0],[0,obj_h-1],[obj_w-1,obj_h-1],[obj_w-1,0] ]).reshape(-1,1,2) + dst = cv2.perspectiveTransform(pts,M) + offset = np.float32([ [x,y],[x,y],[x,y],[x,y] ]).reshape(-1,1,2) + dst = dst + offset + #frame = cv2.bitwise_and(frame, frame, mask = mask) + cv2.polylines(frame, [np.int32(dst)], True, 255, 3) + decorateTime = time.time() - now + cv2.rectangle(frame, (x,y), (x+w, y+h), (0,255,0), 2) + now=time.time() + cv2.imshow("Camera View", frame) + dispTime = time.time() - now + now = time.time() + times = symTimes + (cropTime, histTime, detectTime, matchTime, decorateTime, dispTime, now - cycleTime, now - start) + avgs = avg(times) + print "AVG -> AQU: %f, CVT: %f, IRG: %f, MSK: %f, CTR: %f, RCT: %f, CRP: %f, HST: %f, DCT: %f, MCH %f, DEC: %f, DSP: %f, TTL: %f, SPF: %f" % avgs + maxs = maximum(times) + print "MAX -> AQU: %f, CVT: %f, IRG: %f, MSK: %f, CTR: %f, RCT: %f, CRP: %f, HST: %f, DCT: %f, MCH %f, DEC: %f, DSP: %f, TTL: %f, SPF: %f" % maxs + mins = minimum(times) + print "MIN -> AQU: %f, CVT: %f, IRG: %f, MSK: %f, CTR: %f, RCT: %f, CRP: %f, HST: %f, DCT: %f, MCH %f, DEC: %f, DSP: %f, TTL: %f, SPF: %f" % mins + start = now + if(cv2.waitKey(1) == 27): + break + symFinder.stop() +cv2.destroyAllWindows() +