Add new multithreaded tracking demo. Update existing single threaded …

…tracking demo to maximise performance and minimise latency.

src/gz_piter/MagPi/symbolFinder.py

@@ -0,0 +1,121 @@
+#!/usr/bin/python
+#
+# symbolFinder.py
+#
+# Author: Derek Campbell
+# Date  : 22/10/2014
+#
+#  Copyright 2014  <guzunty@gmail.com>
+#  
+#  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

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,47 +39,63 @@
 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()):
   print("Cannot open camera")
 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()