Monovision Code:

from collections import deque

from imutils.video import VideoStream

import numpy as np

import argparse

import cv2

import imutils

import time

import matplotlib.pyplot as plt

import os

ap = argparse.ArgumentParser()

ap.add_argument("-v", "--video",

help="path to the (optional) video file")

ap.add_argument("-b", "--buffer", type=int, default=64,

help="max buffer size")

ap.add_argument("-dd","--depth",type=int, default=0,

help="Depth data for calibration: Enter a 1 to collect data to csv file. Set to not use background subtraction mask")

ap.add_argument("-d", "--data", type=int, default=0,

help="Record all position, velocity, and acceleration data. Enter a 1 to enable")

ap.add_argument("-p", "--plot", type=int, default=0,

help="Create plot of the radius over time. Enter a 1 to enable")

args = vars(ap.parse_args())

def colordetec():

# If alldata.csv exists delete it

if os.path.exists("alldata.csv"):

os.remove("alldata.csv")

# define the lower and upper boundaries of the "orange"

# Orange Lower for Logitech webcam (0,210,255)

# Orange upper for Logitech webcam (6,255,255)

# Orange Lower for Logitech webcam in basement lighting (0, 175, 0)

# Orange upper for Logitech webcam in basement lighting (15, 236, 255)

# Orange lower for ping pong testing (10, 149, 138)

# Orange upper fo ping pong testing (18, 255, 255)

# OrangeLower for macbook orangeLower = (0, 114, 142)

# orangeUpper for macbook orangeUpper = (9, 235, 255)

orangeLower = (0, 175, 0)

orangeUpper = (15, 236, 255)

# Initialize the points queue

pts = deque(maxlen=args["buffer"])

pts_real = deque(maxlen=args["buffer"])

vel = deque(maxlen=args["buffer"])

accel = deque(maxlen=args["buffer"])

Y_pred = deque(maxlen=4)

key = 0

# if a video path was not supplied, grab the reference

# to the webcam

if not args.get("video", False):

vs = VideoStream(src=2).start()

fps = 30

# otherwise, grab a reference to the video file

else:

vs = cv2.VideoCapture(args["video"])

fps = vs.get(cv2.CAP_PROP_FPS)

# allow the camera or video file to warm up

time.sleep(2.0)

w=0

radius_list = []

backsub = cv2.createBackgroundSubtractorMOG2(detectShadows=False)

# Uncertainty Constants

mx = 5.7970

dmx = 0.0351889

dxconst = dmx/mx

my = 5.893687646

dmy = 0.098733569

dyconst = dmy/my

mz = 100.1086265

dmz = 1.475697878

bz = 0.293936289

dbz = 0.09054497

# Is ball being thrown

ball_thrown = False

threshdist = 30

threshvel = 1.5

movavgthresh =1

time_real = 0

t_init = time.time()

# keep looping

while True:

t0 = time.time()

# grab the current frame

frame = vs.read()

# handle the frame from VideoCapture or VideoStream

frame = frame[1] if args.get("video", False) else frame

# if we are viewing a video and we did not grab a frame,

# then we have reached the end of the video

if frame is None:

break

# Convert frame to HSV color space. Possible frame resizing goes here too.

# frame = cv2.flip(frame,0)

hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

# Get dimensions of the frame. Operation performed for first frame only.

while w == 0:

fshape = frame.shape

midy = int(fshape[0]/2)

midx = int(fshape[1]/2)

w = w+1

# Create gridlines

cv2.line(frame,(midx,0),(midx,fshape[0]),(0,0,255),2)

cv2.line(frame,(0,midy),(fshape[1],midy),(0,0,255),2)

# construct a mask for the color "orange", then perform

# a series of dilations and erosions to remove any small

# blobs left in the mask

mask = cv2.inRange(hsv, orangeLower, orangeUpper)

submask = backsub.apply(frame)

# mask = cv2.erode(mask, None, iterations=2)

# mask = cv2.dilate(mask, None, iterations=2)

# If collecting depth data only use the color mask. Otherwise do a bitwise and operation with the color mask and the background subtraction mask

if args["depth"]:

finalmask = mask

else:

finalmask = mask & submask

# find contours in the mask and initialize the current

# (x, y) center of the ball

cnts = cv2.findContours(finalmask.copy(), cv2.RETR_EXTERNAL,

cv2.CHAIN_APPROX_SIMPLE)

cnts = imutils.grab_contours(cnts)

center = None

# only proceed if at least one contour was found

if len(cnts) > 0:

# find the largest contour in the mask, then use

# it to compute the minimum enclosing circle and

# centroid

c = max(cnts, key=cv2.contourArea)

((x, y), radius) = cv2.minEnclosingCircle(c)

t_fin = time.time()

# only proceed if the radius meets a minimum size

if radius > 10:

# draw the circle and centroid on the frame,

# then update the list of tracked points

cv2.circle(frame, (int(x), int(y)), int(radius),

(0, 255, 255), 2)

# update the points queue

pts.appendleft(((int(x),int(y),radius)))

if args["plot"]:

radius_list.append((radius,t_fin-t_init))

## Moving Robotic arm

# Perhaps make a transform for the position of the ball on the screen to the position of the ball to the robot

# First normalize the center quanities to start at the center of the image

# The catcher should start off in the center of the goal

# Transform from pixel space to real space

if len(pts)>2:

Xintermediate = pts[0][0]-midx

if Xintermediate == 0:

X_real = 0

else:

X_real = mx*np.pi/np.arctan(pts[0][2]/(pts[0][0]-midx))/180

dX = X_real*dxconst

Yintermediate = midy-pts[0][1]

if Yintermediate == 0:

Y_real = 0

else:

Y_real = 5.8937*np.pi/np.arctan(pts[0][2]/(midy-pts[0][1]))/180

dY = Y_real*dyconst

Z_real = mz/pts[0][2] + bz

dZ = np.sqrt((dmz/pts[0][2])**2+(dbz)**2)

# Filter extraneous data points

if 10<X_real or X_real<-10:

X_real = None

if 20<Y_real or Y_real<-20:

Y_real = None

if 50<Z_real or Z_real<.001:

Z_real = None

pts_real.appendleft([[X_real,dX],[Y_real,dY],[Z_real,dZ],time.time()-t_init])

# Derive Velocities

if len(pts_real)>1 and pts_real[0][0][0] and pts_real[1][0][0] and pts_real[0][1][0] and pts_real[1][1][0] and pts_real[0][2][0] and pts_real[1][2][0]:

Vx = (pts_real[0][0][0]-pts_real[1][0][0])/(pts_real[0][3]-pts_real[1][3])

dVx = np.abs(Vx*np.sqrt(pts_real[0][0][1]**2+pts_real[1][0][1]**2)/(pts_real[0][0][0]-pts_real[1][0][0]))

Vy = (pts_real[0][1][0]-pts_real[1][1][0])/(pts_real[0][3]-pts_real[1][3])

dVy = np.abs(Vy*np.sqrt(pts_real[0][1][1]**2+pts_real[1][1][1]**2)/(pts_real[0][1][0]-pts_real[1][1][0]))

Vz = (pts_real[0][2][0]-pts_real[1][2][0])/(pts_real[0][3]-pts_real[1][3])

dVz = np.abs(Vz*np.sqrt(pts_real[0][2][1]**2+pts_real[1][2][1]**2)/(pts_real[0][2][0]-pts_real[1][2][0]))

#Calculate distance between data points

dist = np.sqrt((pts_real[0][0][0]-pts_real[1][0][0])**2+(pts_real[0][1][0]-pts_real[1][1][0])**2+(pts_real[0][2][0]-pts_real[1][2][0])**2)

# Filter extraneous data points

# If the distance between data points is greater than a threshold set velocities and real points to 0

if dist>threshdist:

Vx = None

dVx = None

Vy = None

dVy = None

Vz = None

dVz = None

if Vx and (30<Vx or Vx<-30):

Vx=None

if Vy and (30<Vy or Vy<-30):

Vy=None

if Vz and (0<Vz or Vz<-50):

Vz=None

else:

Vx = None

dVx = None

Vy = None

dVy = None

Vz = None

dVz = None

vel.appendleft([[Vx,dVx],[Vy,dVy],[Vz,dVz],pts_real[0][3]])

#Moving average filter

movavg = 0

N=0

if len(vel)>3:

for i in range(0,4):

if vel[i][1][0]:

movavg += vel[i][1][0]

N+=1

if N==3:

break

if N>0:

movavg = movavg/N

if vel[0][1][0]:

if vel[1][1][0] and abs(vel[1][1][0] - movavg) > movavgthresh:

vel[1][1][0] = None

if len(vel)>2 and vel[0][0][0] and vel[1][0][0] and vel[0][1][0] and vel[1][1][0] and vel[0][2][0] and vel[1][2][0]:

Ax = (vel[0][0][0]-vel[1][0][0])/(vel[0][3]-vel[1][3])

dAx = np.abs(Ax*np.sqrt(vel[0][0][1]**2+vel[1][0][1]**2)/(vel[0][0][0]-vel[1][0][0]))

Ay = (vel[0][1][0]-vel[1][1][0])/(vel[0][3]-vel[1][3])

dAy = np.abs(Ay*np.sqrt(vel[0][1][1]**2+vel[1][1][1]**2)/(vel[0][1][0]-vel[1][1][0]))

Az = (vel[0][2][0]-vel[1][2][0])/(vel[0][3]-vel[1][3])

dAz = np.abs(Ax*np.sqrt(vel[0][2][1]**2+vel[1][2][1]**2)/(vel[0][2][0]-vel[1][2][0]))

# Filter extraneous data points

else:

Ax = None

dAx = None

Ay = None

dAy = None

Az = None

dAz = None

accel.appendleft([[Ax,dAx],[Ay,dAy],[Az,dAz],vel[0][3]])

# Prediction algorithms

if vel[0][1][0] and pts_real[0][1][0]:

# Make a initial prediction based on Y_real

delta_t = vel[0][3] - vel[1][3]

X_pred = None

Y_pred1=pts_real[0][1][0] + vel[0][1][0]*delta_t+0.5*(-32.2)*(delta_t)**2

Z_pred = None

Vy_pred2 = vel[0][1][0]+(-32.2)*delta_t

Y_pred2 = Y_pred1 + Vy_pred2*delta_t+0.5*(-32.2)*(delta_t)**2

Vy_pred3 = Vy_pred2 + (-32.2)*delta_t

Y_pred3 = Y_pred2 + Vy_pred3*delta_t+0.5*(-32.2)*(delta_t)**2

Y_pred.appendleft([Y_pred1,Y_pred2,Y_pred3])

# Make two more predictions based on a point check the error as the points accumulate

else:

Y_pred.appendleft(None)

if len(Y_pred)>3 and len(pts_real)>3 and Y_pred[3] and pts_real[2][1][0] and pts_real[1][1][0] and pts_real[0][1][0]:

error = pts_real[2][1][0] - Y_pred[3][0] + pts_real[1][1][0] - Y_pred[3][1] + pts_real[0][1][0] - Y_pred[3][2]

if vel[0][1][0] and vel[0][2][0] and abs(error) < 0.3 and vel[0][2][0]<-5:

# print('Ball is being Thrown at time: '+ str(pts_real[2][3]))

ball_thrown = True

ball_thrown_time = time.time()

else:

error = None

#posstring = "X=%.3f, Y=%.3f, Z=%.3f" % (X_real,Y_real,Z_real)

#velstring = "Vx=%.3f, Vy=%.3f, Vz=%.3f" % (vel[0][0][0],vel[0][1][0],vel[0][2][0])

#accelstring = "Ax=%.3f, Ay=%.3f, Az=%.3f" % (accel[0][0][0],accel[0][1][0],accel[0][2][0])

#cv2.putText(frame,posstring,(50,25),cv2.FONT_HERSHEY_COMPLEX,.5,(0,255,0))

#cv2.putText(frame,velstring,(50,50),cv2.FONT_HERSHEY_COMPLEX,.5,(0,255,0))

#cv2.putText(frame,accelstring,(50,100),cv2.FONT_HERSHEY_COMPLEX,.5,(0,255,0))

if args["data"]:

f = open('alldata.csv','a')

#f.write(str(pts_real[0][0][0])+','+str(pts_real[0][0][1])+','+str(pts_real[0][1][0])+','+str(pts_real[0][1][1])+','+str(pts_real[0][2][0])+','+str(pts_real[0][2][1])+','+str(vel[0][0][0])+','+str(vel[0][0][1])+','+str(vel[0][1][0])+','+str(vel[0][1][1])+','+str(vel[0][2][0])+','+str(vel[0][2][1])+','+str(accel[0][0][0])+','+str(accel[0][0][1])+','+str(accel[0][1][0])+','+str(accel[0][1][1])+','+str(accel[0][2][0])+','+str(accel[0][2][1])+','+str(accel[0][3]))

if len(vel)>2 and len(Y_pred)>3:

# f.write(str(pts_real[1][0][0] if pts_real[1][0][0] else '') +','+str(X_pred)+','+str(pts_real[1][1][0] if pts_real[1][1][0] else '')+','+str(Y_pred)+','+str(pts_real[1][2][0] if pts_real[1][2][0] else '')+','+str(Z_pred)+','+str(vel[1][0][0] if vel[1][0][0] else '')+','+str(vel[1][1][0] if vel[1][1][0] else '')+','+str(vel[1][2][0] if vel[1][2][0] else '')+','+str(accel[1][0][0] if accel[1][0][0] else '')+','+str(accel[1][1][0] if accel[1][1][0] else '')+','+str(accel[1][2][0] if accel[1][2][0] else '')+','+str(accel[1][3]))

f.write(str(pts_real[3][1][0] if pts_real[3][1][0] else '')+','+str(error if error else '') +','+str(vel[3][1][0] if vel[3][1][0] else '')+','+str(vel[3][3] if vel[3][3] else ''))

f.write('\n')

f.close()

if ball_thrown:

# Check if time of throw is fessible

# If the time that the ball has been thrown is greater that 1 second set ball is thrown to false

if (time.time() - ball_thrown_time) > 1:

ball_thrown = False

# Find the total flight time of the ball : velocity in the Z direction until Z = 0

flight_time = 0

# moving average for the Z velocity over three points looking over a range of 5 points

movavgVz = 0

N=0

M=0

if len(vel)>3:

for i in range(0,4):

if vel[i][2][0]:

movavgVz += vel[i][2][0]

N+=1

if N==3:

movavgVz = movavgVz/N

break

if pts_real[i][2][0]:

M+=1

if M==1:

Zpos = pts_real[i][2][0]

break

if Zpos and movavgVz:

flight_time = -Zpos/movavgVz

#print('Flight time of the ball is: ' + str(flight_time))

# flight time = -position of ball in Z / Velocity of ball in Z assuming ball is being thrown in the air

# Make a predicted trajectory and determine where the ball will land at flight time

pred_traj = []

# Calculate the FPS and print to frame

t1 = time.time()

FPS = "FPS:"+ str(int(1/(t1-t0)))

cv2.putText(frame,FPS,(fshape[1]-100,50),cv2.FONT_HERSHEY_COMPLEX,.5,(0,255,0))

# Collect data in order to calibrate the range as a function of x, y, and r

# Must have background subtraction turned off

if args["depth"]:

key = cv2.waitKey(1)

if key == ord('a'):

f = open('depth_data.csv','a')

temp1 = input("Input the X coordinate: ")

temp2 = input("Input the Y coordinate: ")

temp3 = input("Input the Z coordinate: ")

f.write(str(pts[0][0]) + ',' + str(pts[0][1]) + ',' + str(pts[0][2])+ ',' + str(temp1) + ',' + str(temp2) + ',' +str(temp3))

f.write('\n')

f.close()

key = 0

print('Data Recorded!\n')

time_real += 1/fps

# show the frame to our screen

cv2.imshow("Frame", frame)

key = cv2.waitKey(1) & 0xFF

# if the 'q' key is pressed, stop the loop

if key == ord("q"):

break

# if we are not using a video file, stop the camera video stream

if not args.get("video", False):

vs.stop()

# otherwise, release the camera

else:

vs.release()

# close all windows

cv2.destroyAllWindows()

# Plot the radius over time

if args["plot"]:

for i in radius_list:

plt.plot(i[1],i[0],'ro')

plt.title('Radius over time')

plt.grid()

plt.show()

return(pts)

colordetec()