def points_process_images(images, roi, cam_degree=30, color=True, color_images=[], threshold=220, intrinsics=None, distortion=None): """ extract 3d pixels and colors from either left or right set of images """ angles = [ math.radians(i * (360.00 / len(images))) for i in range(0, len(images)) ] points = [] xypoints = [] w, h = roi[2:4] for i, path in enumerate(images): img = cv.LoadImage(path) if intrinsics and distortion: source = cv.CloneImage(source_color) cv.Undistort2(source, img, intrinsics, distortion) cv.SetImageROI(img, roi) xy = points_max_cols(img, threshold=threshold) xyz = [ points_triangulate((x - (w / 2), y), angles[i], cam_degree) for x, y in xy ] if color: color = cv.LoadImage(color_images[i]) if intrinsics and distortion: source = cv.CloneImage(color) cv.Undistort2(source, color, intrinsics, distortion) cv.SetImageROI(color, roi) colors = [list(color[y, x]) for x, y in xy] [xyz[i].extend([r, g, b]) for i, (b, g, r) in enumerate(colors)] else: xyz = [[x, y, z, 1.0, 1.0, 1.0] for x, y, z in xyz] points.extend(xyz) return points
def barrel_undistort(self, img): try: dst = cv.CreateImage(cv.GetSize(img), cv.IPL_DEPTH_8U, 3) cv.Undistort2(img, dst, self.imat_, self.dmat_) return dst except: return None
def undistort(self, frame): logging.debug("Undistorting a frame") assert frame.width == self.Idistort.width assert frame.height == self.Idistort.height cv.Undistort2(frame, self.Idistort, self.Intrinsic, self.Distortion) return self.Idistort
def pitch_detect(intrinsics, dist_coeffs, dst0): capture = cv.CaptureFromCAM(0) src = cv.QueryFrame(capture) cv.SetImageROI(dst0, (0, 0, 640, 480)) cv.Undistort2(src, dst0, intrinsics, dist_coeffs) cv.SetImageROI(dst0, image_ROI) dst = GetImage(dst0) hsv = cv.CreateImage(size, 8, 3) CvtColor(dst, hsv, CV_RGB2HSV) cv.Split(hsv, hue, sat, val, None) hist = cv.CreateHist([32, 64], CV_HIST_ARRAY, [[0, 180], [0, 256]], 1) cv.CalcHist([hue, sat], hist, 0, None) values = cv.GetMinMaxHistValue(hist) tweak = values[3][0] return tweak
def on_enter_frame(frame): global have_corners, corners, rowcols, intrinsics, distortion w, h = cv.GetSize(frame) img = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 1) cv.CvtColor(frame, img, cv.CV_RGB2GRAY) have_corners, corners = cv.FindChessboardCorners(img, rowcols) if have_corners: cv.DrawChessboardCorners(frame, rowcols, corners, True) if intrinsics and distortion: new = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 3) cv.Undistort2(frame, new, intrinsics, distortion) cv.ShowImage('Calibrated', new) cv.ShowImage('Frame', frame)
def undistort(self, gui, cam, dist): """Undistort file """ gui.setMessage("Undistort movie... get frame count") #open capture file capture = cv.CaptureFromFile(self.filename) frame = cv.QueryFrame(capture) #count... I know it sucks numframes=1 while frame: frame = cv.QueryFrame(capture) numframes +=1 capture = cv.CaptureFromFile(self.filename) frame = cv.QueryFrame(capture) #grab a frame to get some information self.framesize = (frame.width, frame.height) gray = cv.CreateImage((frame.width,frame.height), 8, 1) undistorted_frame = cv.CreateImage((frame.width,frame.height), 8, 3) cv.NamedWindow("Uncalibrated image",cv.CV_WINDOW_NORMAL) cv.NamedWindow("Calibrated image",cv.CV_WINDOW_NORMAL) while frame: cv.Undistort2(frame, undistorted_frame, cam, dist) cv.ResizeWindow("Uncalibrated image",640,480) cv.ResizeWindow("Calibrated image",640,480) cv.ShowImage("Calibrated image",undistorted_frame) cv.ShowImage("Uncalibrated image",frame) cv.WaitKey(0) frame = cv.QueryFrame(capture) #grab a frame to get some information self.points = points self.nframe = nframe gui.setMessage("Analyze end, %d points found" % len(self.points))
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 undistort(self, src, dst): '''Undistort the image from distortion coefficients''' cv.Undistort2(src, dst, self.cv_camera_matrix, self.distortion_coefficients)
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 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)
#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 <<<<<<< HEAD cv.Threshold(hue,ball,thresBall+ballRange, 255,cv.CV_THRESH_TOZERO_INV) cv.Threshold(hue,ball,thresBall-ballRange, 255,cv.CV_THRESH_BINARY) cv.Erode(yellow,yellow,ex2,1)