def camera():
print "# Starting initialization..."
#camera capture
#cap = cv.CaptureFromCAM(0)
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.Zero(intrinsics)
#camera data
intrinsics[0, 0] = 1100.850708957251072
intrinsics[1, 1] = 778.955239997982062
intrinsics[2, 2] = 1.0
intrinsics[0, 2] = 348.898495232253822
intrinsics[1, 2] = 320.213734835526282
dist_coeffs = cv.CreateMat(1, 4, cv.CV_64FC1)
cv.Zero(dist_coeffs)
dist_coeffs[0, 0] = -0.326795877008420
dist_coeffs[0, 1] = 0.139445565548056
dist_coeffs[0, 2] = 0.001245710462327
dist_coeffs[0, 3] = -0.001396618726445
#pFrame = cv.QueryFrame(cap)
print "# intrinsics loaded!"
#prepare memory
capture = cv.CaptureFromCAM(0)
src = cv.QueryFrame(capture)
size = GetSize(src)
dst0 = cv.CreateImage(size, src.depth, src.nChannels)
# bg = cv.LoadImage("00000005.jpg")
image_ROI = (0,70,640,340)
size = (640,340)
red = cv.CreateImage(size, 8, 1)
green = cv.CreateImage(size, 8, 1)
blue = cv.CreateImage(size, 8, 1)
hue = cv.CreateImage(size, 8, 1)
sat = cv.CreateImage(size, 8, 1)
val = cv.CreateImage(size, 8, 1)
ball = cv.CreateImage(size, 8, 1)
yellow = cv.CreateImage(size, 8, 1)
ballx = 0
bally = 0
ballmiss = 0
yellowmiss = 0
bluemiss = 0
dst2 = cv.CreateImage(size, 8, 3)
hsv = cv.CreateImage(size,8,3)
print "# base images created..."
#####------------------ajustment data---------------------###############
#shadow
high = 40
low = 300
#threshold
thresBallInit = 116
thresYellowInit = 94
thresBlueInit = 18
ballRangeInit = 8.0
yellowRangeInit = 5.0
blueRangeInit = 8.0
ballRange = ballRangeInit
yellowRange = yellowRangeInit
blueRange = blueRangeInit
ballMinRange = 1.5
yellowMinRange = 2.5
blueMinRange = 8.0
thresBall = thresBallInit
thresYellow = thresYellowInit
thresBlue = thresBlueInit
#dilate
ex = cv.CreateStructuringElementEx(3,3,1,1,cv.CV_SHAPE_RECT)
ex2 = cv.CreateStructuringElementEx(2,2,1,1,cv.CV_SHAPE_RECT)
ex5 = cv.CreateStructuringElementEx(5,5,1,1,cv.CV_SHAPE_RECT)
#ball
ballcount = 15.0
ballAreaInit = 105.0
ballAreaRangeInit = 60.0
ballArea = ballAreaInit
ballAreaRange = ballAreaRangeInit
ballMinAreaRange = 40.0
ballcompact = 3.0
#blue
bluecount = 30.0
blueAreaInit = 300.0
blueAreaRangeInit = 150.0
blueArea = blueAreaInit
blueAreaRange = blueAreaRangeInit
blueMiniAreaRange = 50.0
bluemaxdepth = 8.0
blueminidepth = 2.5
#yellow
yellowcount = 30.0
yellowAreaInit = 450.0
yellowAreaRangeInit = 200.0
yellowArea = yellowAreaInit
yellowAreaRange = yellowAreaRangeInit
yellowMinAreaRange = 50.0
yellowmaxdepth = 10.0
yellowminidepth = 3.2
#####----------------------------------------
#create window
NamedWindow("camera",cv.CV_WINDOW_AUTOSIZE)
#NamedWindow("ball",cv.CV_WINDOW_AUTOSIZE)
#NamedWindow("yellow",cv.CV_WINDOW_AUTOSIZE)
#NamedWindow("blue",cv.CV_WINDOW_AUTOSIZE)
#NamedWindow("hue",cv.CV_WINDOW_AUTOSIZE)
#NamedWindow("sat",cv.CV_WINDOW_AUTOSIZE)
#NamedWindow("val",cv.CV_WINDOW_AUTOSIZE)
timecount = 0
onesec = time.clock()
storage = cv.CreateMemStorage()
print "# starting capture..."
print ''
capture = cv.CaptureFromCAM(0)
aa = time.clock()
while(True):
timecount = timecount + 1
src = cv.QueryFrame(capture)
#barrel undistortion
cv.Undistort2(src, dst0, intrinsics, dist_coeffs)
#ROI = Region of Interests, crop the image
cv.SetImageROI(dst0,image_ROI)
dst = GetImage(dst0)
CvtColor(dst,hsv,CV_RGB2HSV)
cv.Split(hsv,hue,sat,val,None)
# ShowImage("hue",hue)
# ShowImage("val",val)
# ShowImage("sat",sat)
# BALL
cv.Threshold(hue,ball,thresBallInit+ballRange, 255,cv.CV_THRESH_TOZERO_INV)
cv.Threshold(hue,ball,thresBallInit-ballRange, 255,cv.CV_THRESH_BINARY)
cv.Erode(ball,ball,ex2,1)
cv.Dilate(ball,ball,ex2,1)
ShowImage("ball",ball)
# YELLOW
cv.Threshold(hue,yellow,thresYellowInit+yellowRange,255,cv.CV_THRESH_TOZERO_INV)
cv.Threshold(yellow,yellow,thresYellowInit-yellowRange,255,cv.CV_THRESH_BINARY)
cv.Erode(yellow,yellow,ex2,1)
cv.Dilate(yellow,yellow,ex2,1)
ShowImage("yellow",yellow)
# BLUE
# CvtColor(dst,hsv,CV_BGR2HSV)
# cv.Split(hsv,hue,sat,val,None)
cv.Threshold(hue,blue,thresBlue+blueRange,255,cv.CV_THRESH_BINARY_INV)
# cv.Threshold(blue,blue,4,255,cv.CV_THRESH_BINARY)
cv.Erode(blue,blue,ex2,1)
cv.Dilate(blue,blue,ex2,1)
ShowImage("blue",blue)
cv.Threshold(val,val,80,255,cv.CV_THRESH_BINARY_INV)
cv.Threshold(sat,sat,80,255,cv.CV_THRESH_BINARY_INV)
ShowImage("sat2",sat)
ShowImage("val2",val)
# Removes the walls
Sub(blue,val,blue)
Sub(blue,sat,blue)
Sub(yellow,val,yellow)
Sub(yellow,sat,yellow)
Sub(ball,val,ball)
Sub(ball,sat,ball)
Set2D(ball,4,4,255)
Set2D(blue,4,4,255)
Set2D(yellow,4,4,255)
ShowImage("yellow3",yellow)
ShowImage("ball3",ball)
ShowImage("blue3",blue)
#find ball
seq = cv.FindContours(ball,storage,cv.CV_RETR_LIST, cv.CV_LINK_RUNS)
if seq != None:
count = 0
while (seq != None and count <= ballcount):
count =count + 1
area = cv.ContourArea(seq)+0.01
compact = ArcLength(seq)*ArcLength(seq)/(4*area*math.pi)
if (area < 4 or area > (ballArea+ballAreaRange) or area < (ballArea-ballAreaRange)): #or compact >= ballcompact ):
seq = seq.h_next()
continue
else:
ballx = 0
bally = 0
for p in seq:
ballx = ballx + p[0]
bally = bally + p[1]
ballx = int(float(ballx)/len(seq))
bally = int(float(bally)/len(seq))
###############--------------Auto ajustment
print "ball area %f" %area
print "ball hue: %f" %hue[bally,ballx]
# thresBall = 0.2*hue[bally,ballx]+0.2*thresBall + 0.5*thresBallInit
# ballArea = area
# if(ballRange > ballMinRange):
# ballRange = ballRange -0.1
# if(ballAreaRange > ballMinAreaRange):
# ballAreaRange = ballAreaRange -1.0
cv.Circle(dst,(ballx,bally),4,cv.CV_RGB(255,255,255),2,8,0)
cv.Circle(dst,(ballx,bally),10,cv.CV_RGB(255,0,0),9,8,0)
break
if(count > ballcount or seq == None):
# thresBall = thresBallInit
# ballRange = 0.5*ballRange + 0.5*ballRangeInit
# ballArea = ballArea + 10.0
# ballAreaRange = ballAreaRange + 0.1
print ballAreaRange
ballx = 0
bally = 0
ballmiss = ballmiss + 1
print "\r# error: ball not found "
#find blue
seq = cv.FindContours(blue,storage,cv.CV_RETR_LIST, cv.CV_LINK_RUNS)
if seq != None:
count = 0
while (seq != None and count <= bluecount):
count =count + 1
area = cv.ContourArea(seq)
if(area < blueArea-blueAreaRange or area > blueArea+blueAreaRange):
seq = seq.h_next()
continue
else:
hull = None
convex = None
hull =cv.ConvexHull2(seq,storage)
convex = cv.ConvexityDefects(seq,hull,storage)
if (len(convex) > 1):
convex = sorted(convex , key = lambda(k1,k2,k3,k4):k4)#sort by depth of the convex defect
if (convex[len(convex)-1][3] < blueminidepth or convex[len(convex)-2][3] < blueminidepth or convex[len(convex)-1][3] > bluemaxdepth or convex[len(convex)-2][3] > bluemaxdepth ):
seq = seq.h_next()
continue
else:
#find the T
blue_start1 = convex[len(convex)-1][0]
blue_end1 = convex[len(convex)-1][1]
blue_depth1 = convex[len(convex)-1][2]
#draw the side line of T
blue_start2 = convex[len(convex)-2][0]
blue_end2 = convex[len(convex)-2][1]
blue_depth2 = convex[len(convex)-2][2]
blue_from = ((blue_depth1[0]+blue_depth2[0])/2,(blue_depth1[1]+blue_depth2[1])/2)#calculate the center of robot
#calculate the end of direction vector, the two end point of the smaller distans
if math.hypot(blue_start1[0]-blue_end2[0],blue_start1[1]-blue_end2[1])>math.hypot(blue_end1[0]-blue_start2[0],blue_end1[1]-blue_start2[1]):
blue_to = ((blue_end1[0]+blue_start2[0])/2,(blue_end1[1]+blue_start2[1])/2)
else:
blue_to = ((blue_start1[0]+blue_end2[0])/2,(blue_start1[1]+blue_end2[1])/2)
cv.Line(dst,blue_from,blue_to,cv.CV_RGB(255,0,255),2,8,0)
cv.Circle(dst,blue_from,1,cv.CV_RGB(255,0,0),2,8,0)
cv.Circle(dst,blue_to,1,cv.CV_RGB(0,0,0),2,8,0)
cv.Circle(dst,blue_from,10,cv.CV_RGB(255,0,0),9,8,0)
#######---------------------------Auto Ajusting
print "blue area %f" %area
# print "blue hue: %f" %hue[blue_from[1],blue_from[0]]
# thresBlue = hue[blue_from[1],blue_from[0]] #+ 0.4*thresBlue + 0.5*thresBlueInit
# blueArea = area
# if(blueAreaRange > blueMiniAreaRange):
# blueAreaRange = blueAreaRange -1.0
# if(blueRange > blueMinRange):
# blueRange = blueRange - 0.1
break
else:
seq = seq.h_next()
continue
if(count > bluecount or seq == None):
# thresBlue = thresBlueInit
# blueAreaRange = blueAreaRange + 10.0
# blueArea = blueArea + 10.0
# blueRange = 0.5*blueRange + 0.5*blueRangeInit
bluemiss = bluemiss + 1
blue_from = (0,0);
blue_to = (0,0);
print "\r# error: blue not found "
#find yellow
seq = cv.FindContours(yellow,storage,cv.CV_RETR_LIST, cv.CV_LINK_RUNS)
if seq != None:
count = 0
while (seq != None and count <= yellowcount):
count =count + 1
area = cv.ContourArea(seq)
if(area < yellowArea-yellowAreaRange or area > yellowArea + yellowAreaRange):
seq = seq.h_next()
continue
else:
hull = None
convex = None
hull =cv.ConvexHull2(seq,storage)
convex = cv.ConvexityDefects(seq,hull,storage)
if (len(convex) > 1):
convex = sorted(convex , key = lambda(k1,k2,k3,k4):k4)#sort by depth of the convex defect
if (convex[len(convex)-1][3] < yellowminidepth or convex[len(convex)-2][3] < yellowminidepth or convex[len(convex)-1][3] > yellowmaxdepth or convex[len(convex)-2][3] > yellowmaxdepth ):
seq = seq.h_next()
continue
else:
#find the T
yellow_start1 = convex[len(convex)-1][0]
yellow_end1 = convex[len(convex)-1][1]
yellow_depth1 = convex[len(convex)-1][2]
#draw the side line of T
yellow_start2 = convex[len(convex)-2][0]
yellow_end2 = convex[len(convex)-2][1]
yellow_depth2 = convex[len(convex)-2][2]
yellow_from = ((yellow_depth1[0]+yellow_depth2[0])/2,(yellow_depth1[1]+yellow_depth2[1])/2)#calculate the center of robot
#calculate the end of direction vector, the two end point of the smaller distans
if math.hypot(yellow_start1[0]-yellow_end2[0],yellow_start1[1]-yellow_end2[1])>math.hypot(yellow_end1[0]-yellow_start2[0],yellow_end1[1]-yellow_start2[1]):
yellow_to = ((yellow_end1[0]+yellow_start2[0])/2,(yellow_end1[1]+yellow_start2[1])/2)
else:
yellow_to = ((yellow_start1[0]+yellow_end2[0])/2,(yellow_start1[1]+yellow_end2[1])/2)
###########------------------------------Auto Ajusting
# print cv.ContourArea(seq)
print "yellow area %f" %area
print "yellow hue: %f" %hue[yellow_from[1],yellow_from[0]]
# thresYellow = hue[yellow_from[1],yellow_from[0]] #+ 0.4*thresYellow + 0.5*thresYellowInit
# yellowArea = area
# if(yellowRange > yellowMinRange):
# yellowRange = yellowRange -0.1
# if(yellowAreaRange > yellowMinAreaRange):
# yellowAreaRange = yellowAreaRange - 1.0
# yellow_miss = ((yellow_from[0]+yellow_to[0])/2,(yellow_from[1]+yellow_to[1])/2)
cv.Line(dst,yellow_from,yellow_to,cv.CV_RGB(255,0,255),2,8,0)
cv.Circle(dst,yellow_from,1,cv.CV_RGB(255,0,0),2,8,0)
cv.Circle(dst,yellow_to,1,cv.CV_RGB(0,0,0),2,8,0)
cv.Circle(dst,yellow_from,10,cv.CV_RGB(255,0,0),9,8,0)
break
else:
seq = seq.h_next()
continue
if(count > yellowcount or seq == None):
#thresYellow = thresYellowInit
# yellowRange = 0.5*yellowRange + 0.5*yellowRangeInit
#yellowArea = yellowArea
# yellowAreaRange = yellowAreaRange + 10.0
print "area:%d" %yellowArea
yellowmiss = yellowmiss + 1
yellow_from = (0,0);
yellow_to = (0,0);
print "\r# error: yellow not found"
ballpos = (ballx,bally)
#output(ballpos,blue_from,blue_to,yellow_from,yellow_to)
ShowImage("camera",dst)
cv.SetImageROI(dst0,(0,0,640,480))
# ShowImage("camera",dst0)
if( cv.WaitKey(2) >= 0 ):
bb = time.clock()
print "frame rate: %f" %(timecount/(bb-aa))
print "ball miss rate: %f" %(ballmiss)
print "blue miss rate: %f" %(bluemiss)
print "yellow miss rate: %f" %(yellowmiss)
break;
def shapeAnalysis(mask):
height, width = mask.shape
pixels = height * width
# spatial and central moments
moments = cv.Moments(mask, binary=1)
huMoments = cv.GetHuMoments(moments)
print "Shape hu moments", huMoments
# distances from the gravity point
contour_seq = cv.FindContours(np.array(mask), cv.CreateMemStorage(),
cv.CV_RETR_TREE, cv.CV_CHAIN_APPROX_SIMPLE)
gravity_center = (int(moments.m10 / moments.m00),
int(moments.m01 / moments.m00)) # (x, y)
gx, gy = gravity_center
distances = np.array(
[math.sqrt((gx - x)**2 + (gy - y)**2) for (x, y) in contour_seq])
dist_distri, bin_dist = np.histogram(distances,
bins=10,
range=None,
normed=True)
print "dist distribution", dist_distri
dist_max = np.max(distances)
dist_min = np.min(distances)
dist_ratio_min_max = dist_min / dist_max
print "dist ratio min max", dist_ratio_min_max
dist_mean = np.mean(distances)
dist_std = np.std(distances)
# normalize distance min and max
dist_max = dist_max / pixels
dist_min = dist_min / pixels
dist_mean = dist_mean / pixels
dist_std = dist_std / pixels
print "dist max", dist_max
print "dist min", dist_min
print "dist mean", dist_mean
print "dist std", dist_std
# number of petals
nbPetals = np.sum([
min(x1, x2) < dist_mean < max(x1, x2)
for x1, x2 in zip(distances[:-1], distances[1:])
]) / 2
print "petals", nbPetals
poly_seq = cv.ApproxPoly(contour_seq, cv.CreateMemStorage(),
cv.CV_POLY_APPROX_DP, 2.8)
ppimg = np.zeros(mask.shape)
for (x, y) in poly_seq:
ppimg[y, x] = 255
imsave('/home/cplab/workspace/imageex/src/imageex/static/POLYYYAAAAA.png',
ppimg)
convex_hull = cv.ConvexHull2(poly_seq, cv.CreateMemStorage())
convexity_defects = cv.ConvexityDefects(poly_seq, convex_hull,
cv.CreateMemStorage())
# number of defects
nbDefects = len(convexity_defects)
print "defects", nbDefects
convexity_seq = sum([[cd[0], cd[2], cd[1]] for cd in convexity_defects],
[])
ppimg = np.zeros(mask.shape)
for (x, y) in convexity_seq:
ppimg[y, x] = 255
imsave('/home/cplab/workspace/imageex/src/imageex/static/CONVEXXAAAAA.png',
ppimg)
convexity_depths = np.array([cd[3] for cd in convexity_defects])
convexity_depth_max = np.max(convexity_depths)
convexity_depth_min = np.min(convexity_depths)
convexity_depth_ratio_min_max = convexity_depth_min / convexity_depth_max
print "convexity depth ratio min max", convexity_depth_ratio_min_max
#normalize
convexity_depth_max = convexity_depth_max / pixels
print "convexity depth max", convexity_depth_max
area = cv.ContourArea(contour_seq)
perimeter = cv.ArcLength(contour_seq)
perimeterOarea = perimeter / area
print "perimeter over area", perimeterOarea
features = []
features += list(huMoments)
features += dist_distri, dist_ratio_min_max, dist_max, dist_min, dist_mean, dist_std
features += nbPetals, nbDefects
features += convexity_depth_ratio_min_max, convexity_depth_max, perimeterOarea
return features
def find_convex_defects(contour, hull):
storage = cv.CreateMemStorage(0)
return cv.ConvexityDefects(contour, hull, storage)
contours = cv.FindContours(hsv_mask, cv.CreateMemStorage(),
cv.CV_RETR_LIST, cv.CV_CHAIN_APPROX_SIMPLE,
(0, 0))
des = cv.CreateImage(cv.GetSize(image), 8, 3)
contours2 = False
hull = False
blank = cv.CreateImage(cv.GetSize(image), 8, 3)
while contours:
contours2 = contours
contours = contours.h_next()
if contours2:
hull = cv.ConvexHull2(contours2, cv.CreateMemStorage(),
cv.CV_CLOCKWISE, 0)
defects = cv.ConvexityDefects(contours2, hull, cv.CreateMemStorage())
cv.DrawContours(image, contours2, (255, 0, 0), (0, 255, 0), 0, 5)
noOfDefects = 0
comx, comy = 0, 0
Z, (p, q), radius = cv.MinEnclosingCircle(contours2)
if Z:
cv.Circle(image, (int(p), int(q)), int(5), [0, 255, 255], -1)
for defect in defects:
start, end, far, d = defect
xpos, ypos = far
cv.Line(image, start, end, [0, 255, 0], 2)
if d > 20 and not pointInFace(xpos, ypos, x, y, w, h):
cv.Circle(image, far, 5, [0, 0, 255], -1)
def camera():
found_goals = False
print "# Starting initialization..."
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.Zero(intrinsics)
#camera data
intrinsics[0, 0] = 850.850708957251072
intrinsics[1, 1] = 778.955239997982062
intrinsics[2, 2] = 1
intrinsics[0, 2] = 320.898495232253822
intrinsics[1, 2] = 380.213734835526282
dist_coeffs = cv.CreateMat(1, 4, cv.CV_64FC1)
cv.Zero(dist_coeffs)
dist_coeffs[0, 0] = -0.226795877008420
dist_coeffs[0, 1] = 0.139445565548056
dist_coeffs[0, 2] = 0.001245710462327
dist_coeffs[0, 3] = -0.001396618726445
print "# intrinsics loaded!"
#prepare memory
capture = cv.CaptureFromCAM(0)
src = cv.QueryFrame(capture)
size = GetSize(src)
dst0 = cv.CreateImage(size, src.depth, src.nChannels)
image_ROI = (0, 60, 640, 340)
size = (640, 340)
hue = cv.CreateImage(size, 8, 1)
sat = cv.CreateImage(size, 8, 1)
val = cv.CreateImage(size, 8, 1)
ball = cv.CreateImage(size, 8, 1)
yellow = cv.CreateImage(size, 8, 1)
blue = cv.CreateImage(size, 8, 1)
Set2D(hue, 4, 4, 255)
Set2D(sat, 4, 4, 255)
Set2D(val, 4, 4, 255)
Set2D(ball, 4, 4, 255)
Set2D(yellow, 4, 4, 255)
Set2D(blue, 4, 4, 255)
ballx = 0
bally = 0
print "# base images created..."
#####------------------adjustment data---------------------###############
#shadow
high = 40
low = 300
#threshold
thresred = 160
thresgreen = 220
thresblue = 254
#dilate
ex = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_RECT)
ex2 = cv.CreateStructuringElementEx(2, 2, 1, 1, cv.CV_SHAPE_RECT)
ex5 = cv.CreateStructuringElementEx(5, 5, 1, 1, cv.CV_SHAPE_RECT)
tHack = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_CROSS)
#ball
ballcount = 15
ballmaxarea = 200
ballminiarea = 45
ballcompact = 1.3
#blue
bluecount = 30
bluemaxarea = 1500
blueminiarea = 50
bluemaxdepth = 10
blueminidepth = 2
#yellow
yellowcount = 30
yellowmaxarea = 1000
yellowminiarea = 50
yellowmaxdepth = 10
yellowminidepth = 3.2
#####----------------------------------------
aa = time.time()
storage = cv.CreateMemStorage()
first = True
pitch = 0 # 0 for main pitch, 1 for alt pitch
countf = 0
print "# starting capture..."
print ''
capture = cv.CaptureFromCAM(0)
while (True):
src = cv.QueryFrame(capture)
#ShowImage('src',src)
cv.SetImageROI(dst0, (0, 0, 640, 480))
average = cv.CreateImage(size, 8, 3)
#barrel undistortion
cv.Undistort2(src, dst0, intrinsics, dist_coeffs)
#ROI = Region of Interests, crop the image
cv.SetImageROI(dst0, image_ROI)
dst = GetImage(dst0)
dst2 = cv.CreateImage(size, 8, 3)
Set2D(dst2, 4, 4, 255)
hsv = cv.CreateImage(size, 8, 3)
CvtColor(dst, hsv, CV_RGB2HSV)
cv.Split(hsv, hue, sat, val, None)
if (first):
#hist = cv.CreateHist([32,64], CV_HIST_ARRAY, [[0,180], [0,256]], 1)
#cv.CalcHist([hue, sat], hist, 0, None)
#values = cv.GetMinMaxHistValue(hist)
#print values
#tweak = values[3][0]
#if tweak >= 12:
# pitch = 1
#print ">>> tweak=",tweak,"pitch selected =",pitch
pitch = pitchSet
if pitch == 1:
base = cv.LoadImage("base.jpg", cv.CV_LOAD_IMAGE_UNCHANGED)
baseInv = cv.CreateImage(size, 8, 1)
cv.Not(base, baseInv)
#huecorr = cv.LoadImage("huecorr.jpg",cv.CV_LOAD_IMAGE_UNCHANGED)
#cv.Smooth(huecorr,huecorr)
#ShowImage("base",base)
#base = cv.CreateImage(size,8,1)
#base = GetImage(val)
#cv.Threshold(hue,hue,75,255,cv.CV_THRESH_BINARY_INV)
#cv.SaveImage("huecorr.jpg", hue)
#cv.Threshold(base,base,110,255,cv.CV_THRESH_BINARY)
#cv.SaveImage("base.jpg", base)
#cv.WaitKey(-1)
first = False
if (debug):
ShowImage("hue", hue)
ShowImage("sat", sat)
ShowImage("val", val)
if pitch == 1:
walls = cv.CreateImage(size, 8, 1)
cv.Threshold(val, walls, 50, 255, cv.CV_THRESH_BINARY_INV)
Set2D(walls, 4, 4, 255)
# BALL
# fixed this cause with another robot it was finding the ball on it. seems to work
Add(sat, hue, ball)
Sub(ball, walls, ball)
cv.SubS(ball, 60, ball, baseInv)
cv.Threshold(ball, ball, 170, 255, cv.CV_THRESH_BINARY)
cv.Erode(ball, ball, ex, 1)
cv.Dilate(ball, ball, ex2, 1)
Set2D(ball, 4, 4, 255)
# YELLOW
# cv.Threshold(hue,yellow,80,255,cv.CV_THRESH_BINARY)
cv.Threshold(val, yellow, 250, 255, cv.CV_THRESH_BINARY)
Sub(yellow, walls, yellow)
cv.Erode(yellow, yellow, ex, 1)
Set2D(yellow, 4, 4, 255)
# blue
cv.Add(walls, hue, blue)
cv.Threshold(blue, blue, 40, 255, cv.CV_THRESH_BINARY_INV)
cv.Erode(blue, blue, ex2, 2)
Set2D(blue, 4, 4, 255)
cv.Dilate(blue, blue, tHack, 2)
if pitch == 0:
ballcompact = 2.0
walls = cv.CreateImage(size, 8, 1)
cv.Threshold(val, walls, 50, 255, cv.CV_THRESH_BINARY_INV)
Set2D(walls, 4, 4, 255)
# BALL
#cv.Add(sat,val,ball)
#ShowImage("rawB",ball)
cv.Threshold(hue, ball, 110, 255, cv.CV_THRESH_BINARY)
cv.Erode(ball, ball, ex2, 1)
cv.Dilate(ball, ball, ex, 1)
# YELLOW
cv.Threshold(val, yellow, 240, 255, cv.CV_THRESH_BINARY)
# cv.Threshold(hue,yellow,80,255,cv.CV_THRESH_TOZERO)
# cv.Threshold(yellow,yellow,105,255,cv.CV_THRESH_TOZERO_INV)
# cv.Threshold(yellow,yellow,50,255,cv.CV_THRESH_BINARY)
cv.Erode(yellow, yellow, ex, 1)
cv.Dilate(yellow, yellow, tHack, 1)
# BLUE
CvtColor(dst, hsv, CV_BGR2HSV)
cv.Split(hsv, hue, sat, val, None)
cv.Threshold(hue, blue, 80, 255, cv.CV_THRESH_BINARY)
cv.Threshold(val, val, 80, 255, cv.CV_THRESH_BINARY_INV)
# Removes the walls
Sub(blue, val, blue)
Sub(yellow, val, yellow)
Sub(ball, val, ball)
cv.Erode(blue, blue, ex, 1)
Set2D(ball, 4, 4, 255)
Set2D(yellow, 4, 4, 255)
Set2D(blue, 4, 4, 255)
if (debug):
ShowImage("blue", blue)
ShowImage("yellow", yellow)
ShowImage("ball", ball)
#find ball
#seq = None
seq = cv.FindContours(ball, storage, cv.CV_RETR_LIST, cv.CV_LINK_RUNS)
if seq != None:
count = 0
#print seq
while seq != None:
compact = 0
count = count + 1
if (count > ballcount):
break
#removed and pitch==0 no idea why it was there
if (cv.ContourArea(seq) != 0):
compact = ArcLength(seq) * ArcLength(seq) / (
4 * cv.ContourArea(seq) * math.pi)
if compact >= ballcompact:
print ">> compact: ", compact, ballcompact
seq = seq.h_next()
continue
area = cv.ContourArea(seq)
if (area == 0 or area > ballmaxarea
or area < ballminiarea): # or compact > ballcompact):
print ">> area: ", area, ballmaxarea, ballminiarea
seq = seq.h_next()
continue
else:
ballx = 0
bally = 0
for p in seq:
ballx = ballx + p[0]
bally = bally + p[1]
ballx = int(float(ballx) / len(seq))
bally = int(float(bally) / len(seq))
# print "compact=%f,area=%f" %(compact,area)
cv.Circle(dst, (ballx, bally), 4, cv.CV_RGB(255, 255, 255),
2, 8, 0)
cv.Circle(dst2, (ballx, bally), 4,
cv.CV_RGB(255, 255, 255), 2, 8, 0)
break
if (count > 15 or seq == None):
ballx = -1
bally = -1
print "# error: ball not found "
#find blue
seq = None
seq = cv.FindContours(blue, storage, cv.CV_RETR_LIST, cv.CV_LINK_RUNS)
if seq != None:
count = 0
while seq != None:
count = count + 1
if (count > bluecount):
break
if (cv.ContourArea(seq) < blueminiarea
or cv.ContourArea(seq) > bluemaxarea):
seq = seq.h_next()
continue
else:
hull = None
convex = None
#
hull = cv.ConvexHull2(seq, storage)
convex = cv.ConvexityDefects(seq, hull, storage)
if (len(convex) > 1):
convex = sorted(convex,
key=lambda (k1, k2, k3, k4): k4
) #sort by depth of the convex defect
if (convex[len(convex) - 1][3] < blueminidepth
or convex[len(convex) - 2][3] < blueminidepth
or convex[len(convex) - 1][3] > bluemaxdepth
or convex[len(convex) - 2][3] > bluemaxdepth):
cv.Line(dst, convex[len(convex) - 1][0],
convex[len(convex) - 1][2],
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, convex[len(convex) - 1][2],
convex[len(convex) - 1][1],
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst, convex[len(convex) - 2][0],
convex[len(convex) - 2][2],
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, convex[len(convex) - 2][2],
convex[len(convex) - 2][1],
cv.CV_RGB(0, 255, 255), 2, 8, 0)
seq = seq.h_next()
continue
else:
#find the T
blue_start1 = convex[len(convex) - 1][0]
blue_end1 = convex[len(convex) - 1][1]
blue_depth1 = convex[len(convex) - 1][2]
#draw the side line of T
cv.Line(dst, blue_start1, blue_depth1,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, blue_depth1, blue_end1,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst2, blue_start1, blue_depth1,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst2, blue_depth1, blue_end1,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
blue_start2 = convex[len(convex) - 2][0]
blue_end2 = convex[len(convex) - 2][1]
blue_depth2 = convex[len(convex) - 2][2]
cv.Line(dst, blue_start2, blue_depth2,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, blue_depth2, blue_end2,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst2, blue_start2, blue_depth2,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst2, blue_depth2, blue_end2,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
blue_from = ((blue_depth1[0] + blue_depth2[0]) / 2,
(blue_depth1[1] + blue_depth2[1]) / 2
) #calculate the center of robot
#calculate the end of direction vector, the two end point of the smaller distans
if math.hypot(blue_start1[0] - blue_end2[0],
blue_start1[1] -
blue_end2[1]) > math.hypot(
blue_end1[0] - blue_start2[0],
blue_end1[1] - blue_start2[1]):
blue_to = ((blue_end1[0] + blue_start2[0]) / 2,
(blue_end1[1] + blue_start2[1]) / 2)
else:
blue_to = ((blue_start1[0] + blue_end2[0]) / 2,
(blue_start1[1] + blue_end2[1]) / 2)
cv.Line(dst, blue_from, blue_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst, blue_from, 1, cv.CV_RGB(255, 0, 0),
2, 8, 0)
cv.Circle(dst, blue_to, 1, cv.CV_RGB(0, 0, 0), 2,
8, 0)
cv.Line(dst2, blue_from, blue_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst2, blue_from, 1, cv.CV_RGB(255, 0, 0),
2, 8, 0)
cv.Circle(dst2, blue_to, 1,
cv.CV_RGB(255, 255, 255), 2, 8, 0)
break
else:
seq = seq.h_next()
continue
if (count > bluecount or seq == None):
blue_from = (0, 0)
blue_to = (0, 0)
print "# error: blue not found "
#find yellow
seq = None
seq = cv.FindContours(yellow, storage, cv.CV_RETR_LIST,
cv.CV_LINK_RUNS)
if seq != None:
count = 0
while seq != None:
count = count + 1
if (count > yellowcount):
break
area = cv.ContourArea(seq)
if (area < yellowminiarea or area > yellowmaxarea):
seq = seq.h_next()
continue
else:
hull = None
convex = None
#
hull = cv.ConvexHull2(seq, storage)
convex = cv.ConvexityDefects(seq, hull, storage)
if (len(convex) > 1):
convex = sorted(convex,
key=lambda (k1, k2, k3, k4): k4
) #sort by depth of the convex defect
if (convex[len(convex) - 1][3] < yellowminidepth
or convex[len(convex) - 2][3] < yellowminidepth
or convex[len(convex) - 1][3] > yellowmaxdepth
or
convex[len(convex) - 2][3] > yellowmaxdepth):
seq = seq.h_next()
continue
else:
#find the T
yellow_start1 = convex[len(convex) - 1][0]
yellow_end1 = convex[len(convex) - 1][1]
yellow_depth1 = convex[len(convex) - 1][2]
#draw the side line of T
cv.Line(dst, yellow_start1, yellow_depth1,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, yellow_depth1, yellow_end1,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst2, yellow_start1, yellow_depth1,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst2, yellow_depth1, yellow_end1,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
yellow_start2 = convex[len(convex) - 2][0]
yellow_end2 = convex[len(convex) - 2][1]
yellow_depth2 = convex[len(convex) - 2][2]
cv.Line(dst, yellow_start2, yellow_depth2,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst, yellow_depth2, yellow_end2,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
cv.Line(dst2, yellow_start2, yellow_depth2,
cv.CV_RGB(0, 0, 255), 2, 8, 0)
cv.Line(dst2, yellow_depth2, yellow_end2,
cv.CV_RGB(0, 255, 255), 2, 8, 0)
yellow_from = (
(yellow_depth1[0] + yellow_depth2[0]) / 2,
(yellow_depth1[1] + yellow_depth2[1]) / 2
) #calculate the center of robot
#calculate the end of direction vector, the two end point of the smaller distans
if math.hypot(
yellow_start1[0] - yellow_end2[0],
yellow_start1[1] -
yellow_end2[1]) > math.hypot(
yellow_end1[0] - yellow_start2[0],
yellow_end1[1] - yellow_start2[1]):
yellow_to = (
(yellow_end1[0] + yellow_start2[0]) / 2,
(yellow_end1[1] + yellow_start2[1]) / 2)
else:
yellow_to = (
(yellow_start1[0] + yellow_end2[0]) / 2,
(yellow_start1[1] + yellow_end2[1]) / 2)
# print cv.ContourArea(seq)
cv.Line(dst, yellow_from, yellow_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst, yellow_from, 1,
cv.CV_RGB(255, 0, 0), 2, 8, 0)
cv.Circle(dst, yellow_to, 1, cv.CV_RGB(0, 0, 0), 2,
8, 0)
cv.Line(dst2, yellow_from, yellow_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst2, yellow_from, 1,
cv.CV_RGB(255, 0, 0), 2, 8, 0)
cv.Circle(dst2, yellow_to, 1,
cv.CV_RGB(255, 255, 255), 2, 8, 0)
break
else:
seq = seq.h_next()
continue
if (count > yellowcount or seq == None):
yellow_from = (0, 0)
yellow_to = (0, 0)
print "# error: yellow not found"
ballpos = (ballx, bally)
ShowImage("camera", dst)
if (found_goals == False):
if (us == "yellow"):
goals = find_goals(size, yellow_from)
stewies_goal = goals[0]
loiss_goal = goals[1]
found_goals = True
elif (us == "blue"):
goals = find_goals(size, blue_from)
stewies_goal = goals[0]
loiss_goal = goals[1]
found_goals = True
#if (ballx >= 0):
output(ballpos, blue_from, blue_to, yellow_from, yellow_to,
stewies_goal, loiss_goal)
time_passed = time.time() - aa
countf += 1
if (time_passed >= 1):
print "frame per second: " + str(countf),
countf = 0
aa = time.time()
cv.WaitKey(2)
def test_2542670(self):
xys = [
(94, 121), (94, 122), (93, 123), (92, 123), (91, 124), (91, 125),
(91, 126), (92, 127), (92, 128), (92, 129), (92, 130), (92, 131),
(91, 132), (90, 131), (90, 130), (90, 131), (91, 132), (92, 133),
(92, 134), (93, 135), (94, 136), (94, 137), (94, 138), (95, 139),
(96, 140), (96, 141), (96, 142), (96, 143), (97, 144), (97, 145),
(98, 146), (99, 146), (100, 146), (101, 146), (102, 146),
(103, 146), (104, 146), (105, 146), (106, 146), (107, 146),
(108, 146), (109, 146), (110, 146), (111, 146), (112, 146),
(113, 146), (114, 146), (115, 146), (116, 146), (117, 146),
(118, 146), (119, 146), (120, 146), (121, 146), (122, 146),
(123, 146), (124, 146), (125, 146), (126, 146), (126, 145),
(126, 144), (126, 143), (126, 142), (126, 141), (126, 140),
(127, 139), (127, 138), (127, 137), (127, 136), (127, 135),
(127, 134), (127, 133), (128, 132), (129, 132), (130, 131),
(131, 130), (131, 129), (131, 128), (132, 127), (133, 126),
(134, 125), (134, 124), (135, 123), (136, 122), (136, 121),
(135, 121), (134, 121), (133, 121), (132, 121), (131, 121),
(130, 121), (129, 121), (128, 121), (127, 121), (126, 121),
(125, 121), (124, 121), (123, 121), (122, 121), (121, 121),
(120, 121), (119, 121), (118, 121), (117, 121), (116, 121),
(115, 121), (114, 121), (113, 121), (112, 121), (111, 121),
(110, 121), (109, 121), (108, 121), (107, 121), (106, 121),
(105, 121), (104, 121), (103, 121), (102, 121), (101, 121),
(100, 121), (99, 121), (98, 121), (97, 121), (96, 121), (95, 121)
]
#xys = xys[:12] + xys[16:]
pts = cv.CreateMat(len(xys), 1, cv.CV_32SC2)
for i, (x, y) in enumerate(xys):
pts[i, 0] = (x, y)
storage = cv.CreateMemStorage()
hull = cv.ConvexHull2(pts, storage)
hullp = cv.ConvexHull2(pts, storage, return_points=1)
defects = cv.ConvexityDefects(pts, hull, storage)
vis = cv.CreateImage((1000, 1000), 8, 3)
x0 = min([x for (x, y) in xys]) - 10
x1 = max([x for (x, y) in xys]) + 10
y0 = min([y for (y, y) in xys]) - 10
y1 = max([y for (y, y) in xys]) + 10
def xform(pt):
x, y = pt
return (1000 * (x - x0) / (x1 - x0), 1000 * (y - y0) / (y1 - y0))
for d in defects[:2]:
cv.Zero(vis)
# First draw the defect as a red triangle
cv.FillConvexPoly(vis, [xform(p) for p in d[:3]],
cv.RGB(255, 0, 0))
# Draw the convex hull as a thick green line
for a, b in zip(hullp, hullp[1:]):
cv.Line(vis, xform(a), xform(b), cv.RGB(0, 128, 0), 3)
# Draw the original contour as a white line
for a, b in zip(xys, xys[1:]):
cv.Line(vis, xform(a), xform(b), (255, 255, 255))
self.snap(vis)
def camera():
found_goals = False
print "# Starting initialization..."
intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1)
cv.Zero(intrinsics)
#camera data
intrinsics[0, 0] = 850.850708957251072
intrinsics[1, 1] = 778.955239997982062
intrinsics[2, 2] = 1
intrinsics[0, 2] = 320.898495232253822
intrinsics[1, 2] = 380.213734835526282
dist_coeffs = cv.CreateMat(1, 4, cv.CV_64FC1)
cv.Zero(dist_coeffs)
dist_coeffs[0, 0] = -0.226795877008420
dist_coeffs[0, 1] = 0.139445565548056
dist_coeffs[0, 2] = 0.001245710462327
dist_coeffs[0, 3] = -0.001396618726445
print "# intrinsics loaded!"
#prepare memory
capture = cv.CaptureFromCAM(0)
src = cv.QueryFrame(capture)
size = GetSize(src)
dst0 = cv.CreateImage(size, src.depth, src.nChannels)
image_ROI = (0, 60, 640, 340)
size = (640, 340)
hue = cv.CreateImage(size, 8, 1)
sat = cv.CreateImage(size, 8, 1)
val = cv.CreateImage(size, 8, 1)
ball = cv.CreateImage(size, 8, 1)
yellow = cv.CreateImage(size, 8, 1)
blue = cv.CreateImage(size, 8, 1)
Set2D(hue, 4, 4, 255)
Set2D(sat, 4, 4, 255)
Set2D(val, 4, 4, 255)
Set2D(ball, 4, 4, 255)
Set2D(yellow, 4, 4, 255)
Set2D(blue, 4, 4, 255)
ballx = 0
bally = 0
ballmiss = 0
yellowmiss = 0
bluemiss = 0
print "# base images created..."
#####------------------ajustment data---------------------###############
#shadow
high = 40
low = 300
#threshold
thresBallInit = 116
thresYellowInit = 94
thresBlueInit = 18
ballRangeInit = 8.0
yellowRangeInit = 6.0
blueRangeInit = 8.0
ballRange = ballRangeInit
yellowRange = yellowRangeInit
blueRange = blueRangeInit
ballMinRange = 1.5
yellowMinRange = 1.5
blueMinRange = 8.0
thresBall = thresBallInit
thresYellow = thresYellowInit
thresBlue = thresBlueInit
#dilate
ex = cv.CreateStructuringElementEx(3, 3, 1, 1, cv.CV_SHAPE_RECT)
ex2 = cv.CreateStructuringElementEx(2, 2, 1, 1, cv.CV_SHAPE_RECT)
ex5 = cv.CreateStructuringElementEx(5, 5, 1, 1, cv.CV_SHAPE_RECT)
#ball
ballcount = 15.0
ballAreaInit = 95.0
ballAreaRangeInit = 80.0
ballArea = ballAreaInit
ballAreaRange = ballAreaRangeInit
ballMinAreaRange = 40.0
ballcompact = 8.0
#blue
bluecount = 30.0
blueAreaInit = 400.0
blueAreaRangeInit = 200.0
blueArea = blueAreaInit
blueAreaRange = blueAreaRangeInit
blueMiniAreaRange = 50.0
bluemaxdepth = 9.0
blueminidepth = 2.5
#yellow
yellowcount = 30.0
yellowAreaInit = 450.0
yellowAreaRangeInit = 200.0
yellowArea = yellowAreaInit
yellowAreaRange = yellowAreaRangeInit
yellowMinAreaRange = 50.0
yellowmaxdepth = 10.0
yellowminidepth = 3.2
#####----------------------------------------
aa = time.time()
storage = cv.CreateMemStorage()
first = True
pitch = 0 # 0 for main pitch, 1 for alt pitch
countf = 0
print "# starting capture..."
print ''
capture = cv.CaptureFromCAM(0)
while (True):
global connected
if (not connected):
global s
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
s.connect((hostname, port))
connected = True
except:
print "java down, waiting"
src = cv.QueryFrame(capture)
#ShowImage('src',src)
cv.SetImageROI(dst0, (0, 0, 640, 480))
average = cv.CreateImage(size, 8, 3)
#barrel undistortion
cv.Undistort2(src, dst0, intrinsics, dist_coeffs)
#ROI = Region of Interests, crop the image
cv.SetImageROI(dst0, image_ROI)
dst = GetImage(dst0)
dst2 = cv.CreateImage(size, 8, 3)
Set2D(dst2, 4, 4, 255)
hsv = cv.CreateImage(size, 8, 3)
CvtColor(dst, hsv, CV_RGB2HSV)
cv.Split(hsv, hue, sat, val, None)
if (first):
pitch = pitchSet
if pitch == 1:
base = cv.LoadImage("base.jpg", cv.CV_LOAD_IMAGE_UNCHANGED)
baseInv = cv.CreateImage(size, 8, 1)
cv.Not(base, baseInv)
first = False
if (debug):
ShowImage("hue", hue)
ShowImage("sat", sat)
ShowImage("val", val)
# BALL
cv.Threshold(hue, ball, thresBallInit + ballRange, 255,
cv.CV_THRESH_TOZERO_INV)
cv.Threshold(hue, ball, thresBallInit - ballRange, 255,
cv.CV_THRESH_BINARY)
#ShowImage("ball",ball)
# YELLOW
cv.Threshold(hue, yellow, thresYellowInit + yellowRange, 255,
cv.CV_THRESH_TOZERO_INV)
cv.Threshold(yellow, yellow, thresYellowInit - yellowRange, 255,
cv.CV_THRESH_BINARY)
cv.Erode(yellow, yellow, ex, 1)
cv.Dilate(yellow, yellow, ex, 1)
#ShowImage("yellow",yellow)
# BLUE
# CvtColor(dst,hsv,CV_BGR2HSV)
# cv.Split(hsv,hue,sat,val,None)
cv.Threshold(hue, blue, thresBlue + blueRange, 255,
cv.CV_THRESH_BINARY_INV)
# cv.Threshold(blue,blue,4,255,cv.CV_THRESH_BINARY)
# cv.Erode(blue,blue,ex2,1)
#ShowImage("blue",blue)
cv.Threshold(val, val, 130, 255, cv.CV_THRESH_BINARY_INV)
cv.Threshold(sat, sat, 100, 255, cv.CV_THRESH_BINARY_INV)
#ShowImage("sat2",sat)
#ShowImage("val2",val)
# Removes the walls
Sub(blue, val, blue)
Sub(blue, sat, blue)
Sub(yellow, val, yellow)
Sub(yellow, sat, yellow)
Sub(ball, val, ball)
Sub(ball, sat, ball)
cv.Erode(ball, ball, ex, 1)
cv.Dilate(ball, ball, ex, 1)
cv.Dilate(blue, blue, ex, 1)
Set2D(ball, 4, 4, 255)
Set2D(blue, 4, 4, 255)
Set2D(yellow, 4, 4, 255)
#ShowImage("yellow3",yellow)
#ShowImage("ball3",ball)
#ShowImage("blue3",blue)
if (debug):
ShowImage("blue", blue)
ShowImage("yellow", yellow)
ShowImage("ball", ball)
#find ball
seq = cv.FindContours(ball, storage, cv.CV_RETR_LIST, cv.CV_LINK_RUNS)
if seq != None:
count = 0
while (seq != None and count <= ballcount):
count = count + 1
area = cv.ContourArea(seq) + 0.01
compact = ArcLength(seq) * ArcLength(seq) / (4 * area *
math.pi)
if (area < 4 or area > (ballArea + ballAreaRange) or area <
(ballArea - ballAreaRange) or compact >= ballcompact):
seq = seq.h_next()
continue
else:
ballx = 0
bally = 0
for p in seq:
ballx = ballx + p[0]
bally = bally + p[1]
ballx = int(float(ballx) / len(seq))
bally = int(float(bally) / len(seq))
###############--------------Auto ajustment
# print "ball area %f" %area
# print "ball hue: %f" %hue[bally,ballx]
# cv.Circle(dst,(ballx,bally),4,cv.CV_RGB(255,255,255),2,8,0)
cv.Circle(dst, (ballx, bally), 5, cv.CV_RGB(255, 255, 255),
3, 8, 0)
break
if (count > ballcount or seq == None):
# print ballAreaRange
ballx = 0
bally = 0
ballmiss = ballmiss + 1
print "# error: ball not found "
#find blue
seq = cv.FindContours(blue, storage, cv.CV_RETR_LIST, cv.CV_LINK_RUNS)
if seq != None:
count = 0
while (seq != None and count <= bluecount):
count = count + 1
area = cv.ContourArea(seq)
if (area < blueArea - blueAreaRange
or area > blueArea + blueAreaRange):
seq = seq.h_next()
continue
else:
hull = None
convex = None
hull = cv.ConvexHull2(seq, storage)
convex = cv.ConvexityDefects(seq, hull, storage)
if (len(convex) > 1):
convex = sorted(convex,
key=lambda (k1, k2, k3, k4): k4
) #sort by depth of the convex defect
if (convex[len(convex) - 1][3] < blueminidepth
or convex[len(convex) - 2][3] < blueminidepth
or convex[len(convex) - 1][3] > bluemaxdepth
or convex[len(convex) - 2][3] > bluemaxdepth):
seq = seq.h_next()
continue
else:
#find the T
blue_start1 = convex[len(convex) - 1][0]
blue_end1 = convex[len(convex) - 1][1]
blue_depth1 = convex[len(convex) - 1][2]
#draw the side line of T
blue_start2 = convex[len(convex) - 2][0]
blue_end2 = convex[len(convex) - 2][1]
blue_depth2 = convex[len(convex) - 2][2]
blue_from = ((blue_depth1[0] + blue_depth2[0]) / 2,
(blue_depth1[1] + blue_depth2[1]) / 2
) #calculate the center of robot
#calculate the end of direction vector, the two end point of the smaller distans
if math.hypot(blue_start1[0] - blue_end2[0],
blue_start1[1] -
blue_end2[1]) > math.hypot(
blue_end1[0] - blue_start2[0],
blue_end1[1] - blue_start2[1]):
blue_to = ((blue_end1[0] + blue_start2[0]) / 2,
(blue_end1[1] + blue_start2[1]) / 2)
else:
blue_to = ((blue_start1[0] + blue_end2[0]) / 2,
(blue_start1[1] + blue_end2[1]) / 2)
cv.Line(dst, blue_from, blue_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst, blue_from, 1, cv.CV_RGB(255, 0, 0),
2, 8, 0)
cv.Circle(dst, blue_to, 3, cv.CV_RGB(0, 0, 0), 2,
8, 0)
cv.Circle(dst, blue_from, 5,
cv.CV_RGB(0, 255, 255), 3, 8, 0)
#######---------------------------Auto Ajusting
print "blue area %f" % area
# print "blue hue: %f" %hue[blue_from[1],blue_from[0]]
break
else:
seq = seq.h_next()
continue
if (count > bluecount or seq == None):
bluemiss = bluemiss + 1
blue_from = (0, 0)
blue_to = (0, 0)
print "# error: blue not found "
#find yellow
seq = cv.FindContours(yellow, storage, cv.CV_RETR_LIST,
cv.CV_LINK_RUNS)
if seq != None:
count = 0
while (seq != None and count <= yellowcount):
count = count + 1
area = cv.ContourArea(seq)
if (area < yellowArea - yellowAreaRange
or area > yellowArea + yellowAreaRange):
seq = seq.h_next()
continue
else:
hull = None
convex = None
hull = cv.ConvexHull2(seq, storage)
convex = cv.ConvexityDefects(seq, hull, storage)
if (len(convex) > 1):
convex = sorted(convex,
key=lambda (k1, k2, k3, k4): k4
) #sort by depth of the convex defect
if (convex[len(convex) - 1][3] < yellowminidepth
or convex[len(convex) - 2][3] < yellowminidepth
or convex[len(convex) - 1][3] > yellowmaxdepth
or
convex[len(convex) - 2][3] > yellowmaxdepth):
seq = seq.h_next()
continue
else:
#find the T
yellow_start1 = convex[len(convex) - 1][0]
yellow_end1 = convex[len(convex) - 1][1]
yellow_depth1 = convex[len(convex) - 1][2]
#draw the side line of T
yellow_start2 = convex[len(convex) - 2][0]
yellow_end2 = convex[len(convex) - 2][1]
yellow_depth2 = convex[len(convex) - 2][2]
yellow_from = (
(yellow_depth1[0] + yellow_depth2[0]) / 2,
(yellow_depth1[1] + yellow_depth2[1]) / 2
) #calculate the center of robot
#calculate the end of direction vector, the two end point of the smaller distans
if math.hypot(
yellow_start1[0] - yellow_end2[0],
yellow_start1[1] -
yellow_end2[1]) > math.hypot(
yellow_end1[0] - yellow_start2[0],
yellow_end1[1] - yellow_start2[1]):
yellow_to = (
(yellow_end1[0] + yellow_start2[0]) / 2,
(yellow_end1[1] + yellow_start2[1]) / 2)
else:
yellow_to = (
(yellow_start1[0] + yellow_end2[0]) / 2,
(yellow_start1[1] + yellow_end2[1]) / 2)
###########------------------------------Auto Ajusting
# print cv.ContourArea(seq)
# print "yellow area %f" %area
# print "yellow hue: %f" %hue[yellow_from[1],yellow_from[0]]
cv.Line(dst, yellow_from, yellow_to,
cv.CV_RGB(255, 0, 255), 2, 8, 0)
cv.Circle(dst, yellow_from, 1,
cv.CV_RGB(255, 0, 0), 2, 8, 0)
cv.Circle(dst, yellow_to, 3, cv.CV_RGB(0, 0, 0), 2,
8, 0)
cv.Circle(dst, yellow_from, 5,
cv.CV_RGB(255, 255, 0), 3, 8, 0)
break
else:
seq = seq.h_next()
continue
if (count > yellowcount or seq == None):
yellowmiss = yellowmiss + 1
yellow_from = (0, 0)
yellow_to = (0, 0)
print "# error: yellow not found"
ballpos = (ballx, bally)
ShowImage("camera", dst)
if (found_goals == False):
if (us == "yellow"):
goals = find_goals(size, yellow_from)
stewies_goal = goals[0]
loiss_goal = goals[1]
found_goals = True
elif (us == "blue"):
goals = find_goals(size, blue_from)
stewies_goal = goals[0]
loiss_goal = goals[1]
found_goals = True
#if (ballx >= 0):
output(ballpos, blue_from, blue_to, yellow_from, yellow_to,
stewies_goal, loiss_goal)
time_passed = time.time() - aa
countf += 1
if (time_passed >= 1):
print "frame per second: " + str(countf)
countf = 0
aa = time.time()
keyPress = cv.WaitKey(2)
if (keyPress == 1048608):
break
elif (keyPress >= 0 and keyPress != 1048608):
bb = time.clock()
print "frame rate: %f" % (timecount / (bb - aa))
print "ball miss rate: %f" % (ballmiss)
print "blue miss rate: %f" % (bluemiss)
print "yellow miss rate: %f" % (yellowmiss)
def __init__(self, poly, depth):
self.depth = depth
self.points = []
self.angles = [.0]
self.touching = []
self.children = []
self.parents = []
xavg = []
yavg = []
xmin = ymin = xmax = ymax = None
for j in range(poly.total):
ptr = cv.GetSeqElem(poly, j)
#point = ctypes.cast(_ptr, cv.Point.CAST )
#x = point.contents.x; y = point.contents.y
point = cv.Point(pointer=ptr, cast=True)
x = point.x
y = point.y
p = Point(x, y)
if self.points:
a = math.degrees(self.points[-1].angle(p))
self.angles.append(a)
self.points.append(p)
xavg.append(x)
yavg.append(y)
if j == 0:
xmin = xmax = x
ymin = ymax = y
else:
if x < xmin: xmin = x
if x > xmax: xmax = x
if y < ymin: ymin = y
if y > ymax: ymax = y
self.avariance = .0
self.avariance_points = [.0, .0]
if self.angles:
print(self.angles)
prev = self.angles[0]
for a in self.angles[1:]:
v = abs(prev - a)
self.avariance_points.append(v)
self.avariance += v
prev = a
#print 'variance', self.avariance
#print 'variance-points', self.avariance_points
#print 'len len', len(self.points), len(self.avariance_points)
n = len(self.points)
self.weight = (sum(xavg) / float(n), sum(yavg) / float(n))
self.width = xmax - xmin
self.height = ymax - ymin
self.center = (int(xmin + (self.width / 2)),
int(ymin + (self.height / 2)))
self.rectangle = ((xmin, ymin), (xmax, ymax))
self.dwidth = xmax - xmin
self.dheight = ymax - ymin
self.dcenter = (xmin + (self.dwidth / 2), ymin + (self.dheight / 2))
self.drectangle = ((xmin, ymin), (xmax, ymax))
self.defects = []
self.center_defects = None
self.convex = cv.CheckContourConvexity(poly)
if not self.convex:
T = 80
dxavg = []
dyavg = []
hull = cv.ConvexHull2(poly, self.storage_hull, 1, 0)
defects = cv.ConvexityDefects(poly, hull, self.storage_defects)
n = defects.total
for j in range(n):
D = cv.ConvexityDefect(pointer=cv.GetSeqElem(defects, j),
cast=True)
s = D.start.contents
e = D.end.contents
d = D.depth_point.contents
start = (s.x, s.y)
end = (e.x, e.y)
depth = (d.x, d.y)
## ignore large defects ##
if abs(end[0] -
depth[0]) > T or abs(end[1] - depth[1]) > T or abs(
start[0] - end[0]) > T or abs(start[1] -
end[1]) > T:
continue
dxavg.append(depth[0])
dyavg.append(depth[1])
self.defects.append((start, end, depth))
xmin = ymin = 999999
xmax = ymax = -1
if self.defects:
n = len(self.defects)
self.center_defects = (int(sum(dxavg) / float(n)),
int(sum(dyavg) / float(n)))
for j, f in enumerate(self.defects):
s, e, d = f
if s[0] < xmin: xmin = s[0]
if e[0] < xmin: xmin = e[0]
if s[0] > xmax: xmax = s[0]
if e[0] > xmax: xmax = e[0]
if s[1] < ymin: ymin = s[1]
if e[1] < ymin: ymin = e[1]
if s[1] > ymax: ymax = s[1]
if e[1] > ymax: ymax = e[1]
self.dwidth = xmax - xmin
self.dheight = ymax - ymin
self.dcenter = (xmin + (self.dwidth / 2),
ymin + (self.dheight / 2))
self.drectangle = ((xmin, ymin), (xmax, ymax))
cv.ClearMemStorage(self.storage_hull)
cv.ClearMemStorage(self.storage_defects)
#find blue seq = cv.FindContours(blue,storage,cv.CV_RETR_LIST, cv.CV_LINK_RUNS) if seq != None: count = 0 while (seq != None and count <= bluecount): count =count + 1 area = cv.ContourArea(seq) if(area < blueArea-blueAreaRange or area > blueArea+blueAreaRange): seq = seq.h_next() continue else: hull = None convex = None hull =cv.ConvexHull2(seq,storage) convex = cv.ConvexityDefects(seq,hull,storage) if (len(convex) > 1): convex = sorted(convex , key = lambda(k1,k2,k3,k4):k4)#sort by depth of the convex defect if (convex[len(convex)-1][3] < blueminidepth or convex[len(convex)-2][3] < blueminidepth or convex[len(convex)-1][3] > bluemaxdepth or convex[len(convex)-2][3] > bluemaxdepth ): seq = seq.h_next() continue else: #find the T blue_start1 = convex[len(convex)-1][0] blue_end1 = convex[len(convex)-1][1] blue_depth1 = convex[len(convex)-1][2] #draw the side line of T blue_start2 = convex[len(convex)-2][0]
def repeat(begin, unmute, last, hold, beginhold):
"""Actual finger detection function, passes mute and click status"""
#captures input frame
frame = cv.QueryFrame(capture)
#creates horizontally flipped copy of input frame to work with
cv.Copy(frame, sframe)
cv.Flip(sframe, sframe, 1)
#makes mask of skintones
dog = skin(sframe, ccolor)
#inverts skintone mask to all non-skin areas
cv.ConvertScale(dog, dog, -1, 255)
#makes greyscale copy of frame
cv.CvtColor(sframe, grey, cv.CV_BGR2GRAY)
#replaces nonskin areas with white
cv.Add(grey, white, grey, dog)
#implements laplacian edge detection on greyscale image
dst_16s2 = cv.CreateImage(cv.GetSize(bg), cv.IPL_DEPTH_16S, 1)
cv.Laplace(grey, dst_16s2, 5)
cv.Convert(dst_16s2, grey)
#creates a threshold to binarize the image
cv.Threshold(grey, grey, 75, 255, cv.CV_THRESH_BINARY)
#creates contours on greyscale image
storage = cv.CreateMemStorage(0)
contours = cv.FindContours(grey, storage, cv.CV_RETR_TREE,
cv.CV_CHAIN_APPROX_SIMPLE)
#sets final display frame background to black
cv.Set(cframe, 0)
#sets minimum range for object detection
mx = 20000
#initializes hand position to previous
best = last
#creates some cvSeq maxcont by copying contours
maxcont = contours
#goes through all contours and finds bounding box
while contours:
bound_rect = cv.BoundingRect(list(contours))
#if bounding box area is greater than min range or current max box
if bound_rect[3] * bound_rect[2] > mx:
#sets max to current object, creates position at center of box, and sets display contour to current
mx = bound_rect[3] * bound_rect[2]
maxcont = contours
#goes to next contour
contours = contours.h_next()
#draws largest contour on final frame
cv.DrawContours(cframe, maxcont, 255, 127, 0)
if maxcont:
#creates convex hull of largest contour
chull = cv.ConvexHull2(maxcont, storage, cv.CV_CLOCKWISE, 1)
cv.PolyLine(cframe, [chull], 1, 255)
chulllist = list(chull)
chull = cv.ConvexHull2(maxcont, storage, cv.CV_CLOCKWISE, 0)
cdefects = cv.ConvexityDefects(maxcont, chull, storage)
#filters small convexity defects and draws large ones
truedefects = []
for j in cdefects:
if j[3] > 30:
truedefects.append(j)
cv.Circle(cframe, j[2], 6, 255)
#if hand is in a pointer position, detects tip of convex hull
if cdefects and len(truedefects) < 4:
tipheight = 481
tiploc = 0
for j in chulllist:
if j[1] < tipheight:
tipheight = j[1]
tiploc = chulllist.index(j)
best = chulllist[tiploc]
#keeps last position if movement too quick, or smooths slower movement
xdiff = best[0] - last[0]
ydiff = best[1] - last[1]
dist = math.sqrt(xdiff**2 + ydiff**2)
if dist > 100:
best = last
else:
best = (last[0] + xdiff * .75, last[1] + ydiff * .75)
#draws main position circle
cv.Circle(cframe, (int(best[0]), int(best[1])), 20, 255)
#displays image with contours
cv.ShowImage("w2", cframe)
cv.MoveWindow('w2', 600, 0)
#delay between frame capture
c = cv.WaitKey(10)
if not hold:
#if largest contour covers half the screen
if mx > 153600 / 2:
#begins timer if not yet started
if begin == 0: begin = time.time()
else:
#sets volume to new volume, or 0 if muted
#in Linux
if sysname == True:
os.system('amixer set Master %s' %
(.64 * unmute * (100 - best[1] / 4.8)))
#in Mac
else:
os.system(
'osascript -e \'set volume output volume %s\'' %
(.64 * unmute * (100 - best[1] / 4.8)))
#if 3 seconds have passed, stops timer and switches mute status
if time.time() - begin > 3:
unmute = 1 - unmute
begin = 0
#stops timer and sets volume to new, if unmuted
else:
begin = 0
#in Linux
if sysname == True:
os.system('amixer set Master %s' %
(int(.64 * unmute *
(100 - best[1] / 4.8)) * .75))
#in Mac
else:
os.system('osascript -e \'set volume output volume %s\'' %
(int(.64 * unmute *
(100 - best[1] / 4.8)) * .75))
#returns timer start, mute status, and previous hand position
return (begin, unmute, best, hold, beginhold)
def repeat1(begin, unmute, last, hold, beginhold):
"""actual function for moving and clicking mouse"""
def click_down():
"""Simulates a down click"""
fake_input(d, ButtonPress, 1)
d.sync()
def click_up():
"""Simulates an up click"""
fake_input(d, ButtonRelease, 1)
d.sync()
#captures input frame
frame = cv.QueryFrame(capture)
#initializes mouse behavior
d = Display()
s = d.screen()
root = s.root
#creates horizontally flipped copy of input frame to work with
cv.Copy(frame, sframe)
cv.Flip(sframe, sframe, 1)
#makes mask of skintones
dog = skin(sframe, ccolor)
#inverts skintone mask to all non-skin areas
cv.ConvertScale(dog, dog, -1, 255)
#makes greyscale copy of frame
cv.CvtColor(sframe, grey, cv.CV_BGR2GRAY)
#replaces nonskin areas with white
cv.Add(grey, white, grey, dog)
#implements laplacian edge detection on greyscale image
dst_16s2 = cv.CreateImage(cv.GetSize(bg), cv.IPL_DEPTH_16S, 1)
cv.Laplace(grey, dst_16s2, 5)
cv.Convert(dst_16s2, grey)
#creates a threshold to binarize the image
cv.Threshold(grey, grey, 75, 255, cv.CV_THRESH_BINARY)
#creates contours on greyscale image
storage = cv.CreateMemStorage(0)
contours = cv.FindContours(grey, storage, cv.CV_RETR_TREE,
cv.CV_CHAIN_APPROX_SIMPLE)
#sets final display frame background to black
cv.Set(cframe, 0)
#sets minimum range for object detection
mx = 20000
#initializes hand position to previous
best = last
#creates some cvSeq maxcont by copying contours
maxcont = contours
#goes through all contours and finds bounding box
while contours:
bound_rect = cv.BoundingRect(list(contours))
#if bounding box area is greater than min range or current max box
if bound_rect[3] * bound_rect[2] > mx:
#sets max to current object, creates position at center of box, and sets display contour to current
mx = bound_rect[3] * bound_rect[2]
maxcont = contours
#goes to next contour
contours = contours.h_next()
#draws largest contour on final frame
cv.DrawContours(cframe, maxcont, 255, 127, 0)
if maxcont:
#draws and finds convex hull and convexity defects
chull = cv.ConvexHull2(maxcont, storage, cv.CV_CLOCKWISE, 1)
cv.PolyLine(cframe, [chull], 1, 255)
chulllist = list(chull)
chull = cv.ConvexHull2(maxcont, storage, cv.CV_CLOCKWISE, 0)
cdefects = cv.ConvexityDefects(maxcont, chull, storage)
#filters smaller convexity defects and displays larger ones
truedefects = []
for j in cdefects:
if j[3] > 30:
truedefects.append(j)
cv.Circle(cframe, j[2], 6, 255)
#Finds highest point of convex hull if hand follows smooth vertical shape
if cdefects and len(truedefects) < 4:
tipheight = 481
tiploc = 0
for j in chulllist:
if j[1] < tipheight:
tipheight = j[1]
tiploc = chulllist.index(j)
best = chulllist[tiploc]
#if hand is open, begin click
if len(truedefects) >= 4:
if beginhold == 0:
beginhold = time.time()
else:
#if .05 seconds have passed, clicks down
if (time.time() - beginhold > .05) and not hold:
hold = True
beginhold = 0
click_down()
#unclicks if hand returns to smooth
else:
if hold:
click_up()
hold = False
beginhold = 0
#keeps last position if movement too quick, or smooths slower movement
xdiff = best[0] - last[0]
ydiff = best[1] - last[1]
dist = math.sqrt(xdiff**2 + ydiff**2)
if dist > 100:
best = last
else:
best = (last[0] + xdiff * .75, last[1] + ydiff * .75)
#displays main position circle
cv.Circle(cframe, (int(best[0]), int(best[1])), 20, 255)
#displays image with contours
cv.ShowImage("w2", cframe)
cv.MoveWindow('w2', 500, 0)
#delay between frame capture
c = cv.WaitKey(10)
#Mouse Move/ Bottom Pointer
Dx, Dy = mousedelta(last, best)
root.warp_pointer((best[0] - 320) * 1600 / 600 + 800,
best[1] * 900 / 360)
d.sync()
return (begin, unmute, best, hold, beginhold)
def cdefects(cnt, hull, storage):
#cnt = the contour in question
#hull = the convex hull of cnt
#returns the points on shape cnt that are maximally
#distant to the convex hull. This is basically the indents
return cv.ConvexityDefects(cnt, hull, storage)