# This code was based on rosebrock's code.
# http://www.computervisiononline.com/blog/tutorial-using-camshift-track-objects-video
# And we used simondlevy's code.
# https://github.com/simondlevy/OpenCV-Python-Hacks/tree/master/Kalman2D
#import the necessary packages
import serial
import string
import numpy as np
import argparse
import cv2
import time
import picamera
import picamera.array
from datetime import datetime
from datetime import time
from datetime import timedelta
from PIL import Image
from sys import exit
from kalman2d import Kalman2D
#initialize the current frame of the video
#and define global variables
frame = None
roiPts = []
inputMode = False
Width = 480
Height = 480
wl = Width*4.5/10
wr = Width*5.5/10
ht = Height*4.5/10
hb = Height*5.5/10
targetBox = np.array([[wl,ht], [wr,ht], [wr,hb], [wl,hb]])
ser = serial.Serial('/dev/ttyAMA0', 9600, timeout=1)
ser.open()
#Center Information
class CenterInfo(object):
def __init__(self): #Init center information
self.x, self.y = -1, -1
def __str__(self): #for print center information
return '%4d %4d' % (self.x, self.y)
#Select RoiBox points from mouse click
def selectROI(event, x, y, flags, param):
global frame, roiPts, inputMode
if inputMode and event == cv2.EVENT_LBUTTONDOWN and len(roiPts) < 4:
roiPts.append((x,y))
cv2.circle(frame,(x,y),4,(0,255,0),2)
cv2.imshow("frame",frame)
#main
def main():
global frame, roiPts, inputMode, roiBoxWidth, roiBoxHeight
cnt = 0 #count for new roiBox from kalmanfilter
centerX = 0
centerY = 0
toggle = True #toggle for imshow
flag = False #flag for moving
kalman2d = Kalman2D() #Create new Kalman filter and initailize
cv2.namedWindow("frame") #window name
cv2.setMouseCallback("frame",selectROI) #mouseCallback
termination = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT,10,3) #camshift termination condition
roiBox = None
time2 = datetime.now() #<<<check time>>>
serial_flag_bit = 1 #Sending data flag
delta = timedelta(seconds = 1) #sleep time for init RoiBox
#Using capture_continuous by defining with 'with'
with picamera.PiCamera() as camera:
stream = picamera.array.PiRGBArray(camera)
camera.resolution = (Width,Height) #set resolution
time3 = datetime.now()
try:
for foo in enumerate(camera.capture_continuous(stream,'bgr',use_video_port = True)): #capture from capture_continuous
time1 = datetime.now() #<<<check time>>>
print(time1 - time2) #period time of 1 cycle of loop
time2 = time1 #<<<check time>>>
frame = stream.array #save image array to variable
stream.seek(0) #initialize stream for next iteration
stream.truncate()
if roiBox is not None:# define roiBox
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) #change image color RGB to HSV
backProj = cv2.calcBackProject([hsv],[0],roiHist, [0,180], 1) #make backprojection
if roiBoxWidth > 0 and roiBoxHeight > 0: #Check camshift is failed
(r,roiBox) = cv2.CamShift(backProj, roiBox, termination) #Do camshift
#r is set of 4points about roiBox
#roiBox is an array, consist of x and y of topLeft, and box's width and height
roiBoxWidth = roiBox[2]
roiBoxHeight = roiBox[3]
serial_flag_bit = 1
else :
#Init roiBox when camshift was failed
print "roiBox init!!!!!!!!!!!!!!!!"
tl[0] = 1
tl[1] = 1
br[0] = Width -1
br[1] = Height -1
roiBox = (tl[0], tl[1], br[0], br[1])
#Camshift
(r,roiBox) = cv2.CamShift(backProj, roiBox, termination)
roiBoxWidth = roiBox[2]
roiBoxHeight = roiBox[3]
serial_flag_bit = 0
time3 = datetime.now()
#transform r to pts to draw Box with polylines
pts = np.int0(cv2.cv.BoxPoints(r))
#Draw roiBox
cv2.polylines(frame,[pts],True, (0,255,0),2)
#Calculate center x,y
centerX = (pts[0][0] + pts[2][0])/2
centerY = (pts[0][1] + pts[2][1])/2
#Update x,y to kalman filter and get estimated x,y
CenterInfo.x = centerX
CenterInfo.y = centerY
#Send center x,y to Arduino
if CenterInfo.x / 10 == 0:
tempCenterX = '00' + str(CenterInfo.x)
elif CenterInfo.x / 100 == 0:
tempCenterX = '0' + str(CenterInfo.x)
else:
tempCenterX = str(CenterInfo.x)
if CenterInfo.y / 10 == 0:
tempCenterY = '00' + str(CenterInfo.y)
elif CenterInfo.y / 100 == 0:
tempCenterY = '0' + str(CenterInfo.y)
else:
tempCenterY = str(CenterInfo.y)
centerData = str(int(flag)) + tempCenterX + tempCenterY
print centerData
#check the time and flag to send information to Arduino. (Data is not sent until delta seconds)
if datetime.now() > time3 + delta :
if serial_flag_bit == 1:
ser.write('%s' %centerData)
#Update Kalman
kalman2d.update(CenterInfo.x, CenterInfo.y)
estimated = [int (c) for c in kalman2d.getEstimate()]
estimatedX = estimated[0]
estimatedY = estimated[1]
#Calculate delta x,y
deltaX = estimatedX - centerX
deltaY = estimatedY - centerY
#Apply new roiBox from kalmanfilter
if cnt > 1:
roiBox = (roiBox[0]+deltaX, roiBox[1]+deltaY, br[0], br[1])
cnt = 0
#Draw estimated center x,y from kalman filter for test
#cv2.circle(frame,(estimatedX,estimatedY), 4, (0, 255, 255),2)
#Change a color when target is in target box
if wl < centerX and wr > centerX and centerY < hb and centerY > ht :
cv2.circle(frame,(centerX,centerY), 4, (255,0,0),2)
flag = False
else :
cv2.circle(frame,(centerX,centerY), 4, (0,255,0),2)
flag = True
cnt = cnt+1 #count for apply new box from kalman filter
#Draw kalman top left point for test
#cv2.circle(frame,(roiBox[0],roiBox[1]), 4, (0,0,255),2)
#Draw target box
cv2.circle(frame,(Width/2,Height/2) , 4, (255,255,255),2)
cv2.polylines(frame, np.int32([targetBox]), 1, (255,255,255))
#Show or unshow imshow
if toggle is True:
cv2.imshow("frame",frame) #if you want speed up, delete this line
key=cv2.waitKey(1) & 0xFF
#Init roiBox
if key == ord("i") and len(roiPts) < 4:
inputMode = True
orig = frame.copy()
#wait for 4th click
while len(roiPts) < 4:
cv2.imshow("frame",frame)
cv2.waitKey(0)
#set data from 4 clicks
roiPts = np.array(roiPts)
s = roiPts.sum(axis=1)
tl = roiPts[np.argmin(s)]
br = roiPts[np.argmax(s)]
#make color histogram from roi
roi = orig[tl[1]:br[1], tl[0]:br[0]]
roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
roiHist = cv2.calcHist([roi],[0],None,[16],[0,180])
roiHist = cv2.normalize(roiHist, roiHist,0,255,cv2.NORM_MINMAX)
roiBox = (tl[0], tl[1], br[0], br[1])
#Set roiBox width, height
roiBoxWidth = roiBox[2]
roiBoxHeight = roiBox[3]
#Calculate center x,y
CenterInfo.x = tl[0]+br[0]/2
CenterInfo.y = tl[1]+br[1]/2
#Init x,y to kalman filter and set first roiBox
CenterInfo.x = centerX
CenterInfo.y = centerY
kalman2d.update(CenterInfo.x, CenterInfo.y)
estimated = [int (c) for c in kalman2d.getEstimate()]
estimatedX = estimated[0]
estimatedY = estimated[1]
#Calculate delta x,y
deltaX = estimatedX - centerX
deltaY = estimatedY - centerY
#set first roiBox
roiBox = (tl[0]+deltaX, tl[1]+deltaY, br[0], br[1])
#toggle for imshow
elif key == ord("r"):
toggle = not toggle
#quit
elif key == ord("q"):
break
finally:
cv2.destroyAllWindows()
if __name__== "__main__":
main()
