def visualizeFunction(self, img, msg): # # Render # colorImage = np.zeros( img.shape, dtype = np.uint8 ) # # colorImage[ img[:,:,1] == 1 ] = [0, 128, 0] # colorImage[ img[:,:,1] == 3 ] = [0, 128, 255] # colorImage[ img[:,:,1] == 8 ] = [255, 255, 255] # # marker # marker = np.zeros( img[ :, :, 0 ].shape, dtype = np.uint8 ) # marker[ img[:,:,1] == 8 ] = 1 # find boundary fieldBoundary, fieldMask = findBoundary(img[:, :, 1], 1, flip=False) # # find circular shape # whiteContour = self.detectCircular( whiteContours ) # cv2.drawContours( colorImage, whiteContour, -1, ( 0, 0, 255 ), 3 ) # # draw region boundary of field # img[ :, :, : ] = colorImage.copy() colorImage = np.zeros(img.shape, dtype=np.uint8) colorImage[img[:, :, 1] == 3] = [0, 128, 255] colorImage[img[:, :, 1] == 8] = [255, 255, 255] colorImage[img[:, :, 1] == 1] = [0, 128, 0] colorImage[:, :, 0] *= fieldMask cv2.drawContours(colorImage, [fieldBoundary], 0, (255, 0, 0), 2) img[:, :, :] = colorImage.copy() if msg.ball_confidence: rospy.logdebug(" error X : {}, error Y : {} ".format( msg.ball_error[0], msg.ball_error[1])) cv2.circle(img, (msg.ball[0], msg.ball[1]), 10, (255, 0, 0), -1) cv2.circle(img, (msg.imgW / 2, msg.imgH / 2), 5, (0, 0, 255), -1)
# create colordef msg colorDef = createColorDefFromDict(colorList) while True: #img = imageList[ indexIndicator ] ret, img = cap.read() #blur = cv2.bilateralFilter( img, 9, 75, 75 ) blurGaussian = cv2.GaussianBlur(img, (5, 5), 0) # get marker marker = colorSegmentation(blurGaussian, colorDef) # get field boundary fieldBoundary, fieldMask = findBoundary(marker, 2, flip=False) fieldMask *= 255 # white marker to white mask whiteImageObject = np.zeros(marker.shape) whiteImageObject[marker == 5] = 1 # and operation with field mask fieldOnly = cv2.bitwise_and(blurGaussian, blurGaussian, mask=fieldMask) whiteObject = (fieldMask * whiteImageObject).astype(np.uint8) minVal = cv2.getTrackbarPos('Min', 'edge') maxVal = cv2.getTrackbarPos('Max', 'edge') #edgeNormal = cv2.Canny( img, 100, 200 ) edgeGaussian = cv2.Canny(fieldOnly, 100, 200)
def main(): # define usage of programing programUsage = "python %prog arg [option] [framePathStr] [modelPathStr] " + str( VERSIONNUMBER) + ', Copyright (C) 2018 FIBO/KMUTT' # initial parser instance parser = optparse.OptionParser(usage=programUsage, description=PROGRAM_DESCRIPTION) # add option of main script parser.add_option("--colorConfig", dest="colorConfig", action='store', type='string', help="Specify color config.", default='../../config/color_config/colordef_test.ini') # add option (options, args) = parser.parse_args() # check number of argument from NUM_REQUIRE_ARGUMENT if len(args) != NUM_REQUIRE_ARGUMENT: # raise error from parser parser.error("require {} argument(s)".format(NUM_REQUIRE_ARGUMENT)) ######################################################### # # get option and argument # framePathStr = args[0] modelPathStr = args[1] colorConfigPathStr = options.colorConfig # get abs path frameAbsPathStr = getImageList(framePathStr) # get config #colorDict = readConfig( 'color_2.ini' ) colorList = getColorListFromConfig(colorConfigPathStr) # create colordef msg colorDef = createColorDefFromDict(colorList) # load image sequence frameList = map(cv2.imread, frameAbsPathStr) # initial index frame idxFrameInt = 0 # initial finalPosition of football finalPosition = None # ball confidence ballConfidence = 0.70 # get model model = loadModel(modelPathStr) # initial hog/svm instance for classifier # FYI : float is confidence predictor = HOG_SVM(modelPathStr, ballConfidence) while True: # get image img = frameList[idxFrameInt] # blur image blurImage = cv2.blur(img, (5, 5)) # get marker marker = colorSegmentation(blurImage, colorDef) # get only white object from marker # NOTE : # Change id of white marker, follow chattarin colordef template # ID White : 5, old : 8 # ID Green : 2, old : 1 whiteImageObject = np.zeros(marker.shape) whiteImageObject[marker == 5] = 1 # get field boundary fieldBoundary, fieldMask = findBoundary(marker, 2, flip=False) # get mask only ball whiteObjectMask = fieldMask * whiteImageObject whiteObjectMask *= 255 # change back to uint8 (opencv support only uint8) whiteObjectMask = whiteObjectMask.astype(np.uint8) # get contours from white object whiteContours = cv2.findContours(whiteObjectMask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)[1] # copy image for visualize visualizeImage = img.copy() # extract feature all white feature extractStatus = predictor.extractFeature(img, whiteContours, objectPointLocation='bottom') if extractStatus == True: # predict all white feature, which the ball is ? predictor.predict() # # get bounding box list from cv2.boundingRect # boundingBoxList = map( cv2.boundingRect, whiteContours ) # # filter # filterFunction = lambda boundingBoxTuple : boundingBoxTuple[ 2 ] >= 10 and boundingBoxTuple[ 3 ] >= 10 # boundingBoxFilterdList = filter( filterFunction, boundingBoxList ) # # filter again check rectangle # filterNonRectFunction = lambda boundingBoxTuple : abs( boundingBoxTuple[ 2 ] - boundingBoxTuple[ 3 ] ) <= 20 # boundingBoxRectList = filter( filterNonRectFunction, boundingBoxFilterdList ) # print len( boundingBoxRectList ) # for boundingBox in boundingBoxRectList: # x1Actual, y1Actual, x2Actual, y2Actual = expandAreaBoundingBox( 10, boundingBox, img.shape[ 1 ], img.shape[ 0 ] ) # # cv2.rectangle( visualizeImage, ( x, y ), # # ( x + width, y + height ), ( 255, 0, 0 ), 2 ) # cv2.rectangle( visualizeImage, ( x1Actual, y1Actual ), # ( x2Actual, y2Actual ), ( 0, 255, 255 ), 2 ) # # get roi # roiCandidateImage = img[ y1Actual : y2Actual, x1Actual : x2Actual ].copy() # # resize # roiCandidateResizedImage = cv2.resize( roiCandidateImage, ( 40, 40 ) ) # # extract feature # featureVector = extractFeatureHog( roiCandidateResizedImage ).T # # predict # predictClass = model.predict( featureVector ) # if predictClass[ 0 ] == 1: # cv2.circle( visualizeImage, ( boundingBox[ 0 ] + boundingBox[ 3 ] / 2, boundingBox[ 1 ] + boundingBox[ 3 ] / 2 ), # boundingBox[ 3 ] , ( 0, 255, 0 ), 2 ) # # visualization zone # for boundingBoxObject in predictor.boundingBoxListObject.boundingBoxList: # cv2.rectangle( visualizeImage, boundingBoxObject.topLeftPositionTuple, # boundingBoxObject.bottomRightPositionTuple, ( 255, 0, 0 ), 2 ) if boundingBoxObject.footballProbabilityScore > ballConfidence: cv2.rectangle(visualizeImage, boundingBoxObject.topLeftPositionTuple, boundingBoxObject.bottomRightPositionTuple, (255, 0, 0), 2) cv2.circle(visualizeImage, boundingBoxObject.object2DPosTuple, 5, (0, 255, 0), -1) else: cv2.rectangle(visualizeImage, boundingBoxObject.topLeftPositionTuple, boundingBoxObject.bottomRightPositionTuple, (0, 0, 255), 2) cv2.circle(visualizeImage, boundingBoxObject.object2DPosTuple, 5, (0, 255, 0), -1) cv2.putText( visualizeImage, "{0:.4f}".format( boundingBoxObject.footballProbabilityScore), boundingBoxObject.topLeftPositionTuple, cv2.FONT_HERSHEY_COMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA) # get best region finalPosition = predictor.getBestRegion() if finalPosition is not None and len(finalPosition) != 0: cv2.circle(visualizeImage, tuple(finalPosition), 10, (255, 0, 0), -1) #print idxFrameInt predictor.boundingBoxListObject.clearBoundingBoxList() # draw contours cv2.drawContours(visualizeImage, [fieldBoundary], 0, (128, 0, 255), 1) # show image cv2.imshow("show", visualizeImage) cv2.imshow("ball mask", whiteObjectMask) # reset final position finalPosition = None # waitkey and break out of the loop k = cv2.waitKey(50) if k == ord('q'): break elif k == ord('d'): idxFrameInt += 1 elif k == ord('a'): idxFrameInt -= 1 # # increment index # idxFrameInt += 1 # # reset frame index if idxFrameInt >= len(frameList): idxFrameInt = len(frameList) - 1 elif idxFrameInt <= 0: idxFrameInt = 0 cv2.destroyAllWindows()
def ImageProcessingFunction(self, img, header): ''' ImageProcessingFunction ''' # get image property imageHeight = img.shape[0] imageWidth = img.shape[1] # get gray scale image imageGray = img[:, :, 0].copy() # initial ball position bestPosition = Point32() ballErrorList = Point32() ballConfidence = 0.0 # get marker (on channel 1) marker = img[:, :, 1] # get field boundary fieldContour, fieldMask = findBoundary(marker, 2, flip=False) self.contourFieldVis = fieldContour # # do ransac # coeffList = findLinearEqOfFieldBoundary(fieldContour[1:-1].copy()) if len(coeffList) > 1: # find y intersect xIntersect = coeffList[0][3] m = coeffList[0][0] c = coeffList[0][1] yIntersect = (m * xIntersect) + c intersectPoint = Point32(x=xIntersect, y=yIntersect, z=0.0) intersectPointConfidence = 1.0 else: intersectPoint = Point32() intersectPointConfidence = 0.0 ransacContour = findNewLineFromRansac(fieldContour, imageWidth, imageHeight) # # cut first and last point # fieldContourMiddlePoints = fieldContour[ 1:-1 ].copy() # # # get ransac result # linearCoefficiant = findLinearEqOfFieldBoundary( fieldContourMiddlePoints ) # # # create list # pointList = list() # # for lineCoeff in linearCoefficiant: # # # get linear coeff # x0 = lineCoeff[ 2 ] # xf = lineCoeff[ 3 ] # m = lineCoeff[ 0 ] # c = lineCoeff[ 1 ] # # # calculate y from x # x = np.arange( x0, xf, dtype = np.int64 ) # y = np.int64( m * x ) + int( c ) # # contour = np.vstack( ( x, y ) ).transpose() # contour = contour.reshape( -1, 1, 2 ) # # pointList.append( contour ) # # if len( pointList ) > 1: # ransacContour = np.vstack( pointList ) ## print pointList[ 0 ].shape ## print pointList[ 1 ].shape ## print contour.shape # else: # ransacContour = pointList[ 0 ] # # # concat first and last point # firstPoint = np.array( [ [ [ 0, img.shape[ 0 ] - 1 ] ] ] ) # lastPoint = np.array( [ [ [ img.shape[ 1 ] - 1, img.shape[ 0 ] - 1 ] ] ] ) # ransacContour = np.concatenate( ( firstPoint, ransacContour, lastPoint ) ) self.ransacContourVis = ransacContour # get new field boundaty from ransac technic newFieldMask = np.zeros(marker.shape, dtype=np.uint8) cv2.drawContours(newFieldMask, [ransacContour], 0, 1, -1) # # get white object and predict ball # whiteObject = np.zeros(marker.shape, dtype=np.uint8) whiteObject[marker == 5] = 1 # get white object only the field whiteObjectInField = whiteObject * newFieldMask whiteObjectInField *= 255 # find contour from white object in field whiteContours = cv2.findContours(whiteObjectInField, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)[1] # # detect goal # goal = findGoal(ransacContour, marker) # goalLeft, goalRight = None, None # # if goalLeft is not None and goalRight is not None: # # # find middle point of goalpost # goalLeftPoint = ( ( ( goalLeft[ 0 ][ 0 ] + goalLeft[ 1 ][ 0 ] ) / 2 ), ( ( goalLeft[ 0 ][ 1 ] + goalLeft[ 1 ][ 1 ] ) / 2 ) ) # goalRightPoint = ( ( ( goalRight[ 0 ][ 0 ] + goalRight[ 1 ][ 0 ] ) / 2 ), ( ( goalRight[ 0 ][ 1 ] + goalRight[ 1 ][ 1 ] ) / 2 ) ) # # goalLeftPoint = Point32( goalLeftPoint[ 0 ], goalRightPoint[ 1 ], 0.0 ) # goalRightPoint = Point32( goalRightPoint[ 0 ], goalRightPoint[ 1 ], 0.0 ) # # goalLeftConfidence = 1.0 # goalRightConfidence = 1.0 # else: # goalLeftPoint = Point32() # goalRightPoint = Point32() # # goalLeftConfidence = 0.0 # goalRightConfidence = 0.0 # # detect goal # # extract feature and predict it extractStatus = self.predictor.extractFeature( imageGray, whiteContours, objectPointLocation='bottom') # check extract status if extractStatus == True: # predict si wait a rai self.predictor.predict() bestPositionList = self.predictor.getBestRegion() if len(bestPositionList) != 0: # convert to point32 bestPosition = Point32(bestPositionList[0], bestPositionList[1], 0.0) # get bounding box object not only position bestBounding = self.predictor.getBestBoundingBox() # get another point of object centerX, centerY = bestBounding.calculateObjectPoint('center') # calculate error errorX, errorY = self.calculateError(imageWidth, imageHeight, centerX, centerY) ballErrorList = Point32(errorX, errorY, 0.0) # ball confidence ballConfidence = 1.0 # # create msg # msg = visionMsg() # assign value to message msg.object_name = [ 'ball', ] msg.pos2D = [ bestPosition, ] msg.imgH = imageHeight msg.imgW = imageWidth msg.object_error = [ballErrorList] msg.object_confidence = [ballConfidence] msg.header.stamp = rospy.Time.now() if goal is not None: for idx, g in enumerate(goal): msg.object_name.append('goal_{}'.format(idx)) x, y = g[0], g[1] x -= 3 while y < 480 and marker[y, x] != 2: y += 1 msg.pos2D.append(Point32(x=float(x), y=float(y), z=0)) # calculate error errorX, errorY = self.calculateError(imageWidth, imageHeight, g[0], g[1]) msg.object_error.append(Point32(x=errorX, y=errorY, z=0)) msg.object_confidence.append(1.0) # append intersect point msg.object_name.append('intersect_point') errorX, errorY = self.calculateError(imageWidth, imageHeight, intersectPoint.x, intersectPoint.y) msg.object_error.append(Point32(x=errorX, y=errorY, z=0.0)) msg.pos2D.append(intersectPoint) msg.object_confidence.append(intersectPointConfidence) return msg
def ImageProcessingFunction(self, img, header): ''' ImageProcessingFunction function ''' startTime = time.time() # Re-initialize self.boundingBoxList = list() # ROI image list imageROIList = list() # Position list -> convert to numpy after that positionList = list() scoreDict = dict() # Get property of image imageHeight = img.shape[0] imageWidth = img.shape[1] # Get grayscale image grayImage = img[:, :, 0].copy() # Get marker marker = img[:, :, 1].copy() # Find field boundary fieldContour, fieldMask = findBoundary(marker, FieldGreenColorID) # Get new contour from ransac ransacContours = findNewLineFromRansac(fieldContour, imageWidth, imageHeight) # Create mask from new contour newFieldMask = np.zeros(marker.shape, dtype=np.uint8) cv2.drawContours(newFieldMask, [ransacContours], 0, 1, -1) # Get white object mask whiteObjectMask = np.zeros(marker.shape, dtype=np.uint8) whiteObjectMask[marker == WhiteColotID] = 1 # Eliminate outer field whiteObjectInFieldMask = whiteObjectMask * newFieldMask * 255 # TRY!!! # Opening for eliminate white field kernel = np.ones(DefaultKernelShapeTuple, dtype=np.uint8) whiteObjectInFieldMask = cv2.morphologyEx(whiteObjectInFieldMask, cv2.MORPH_OPEN, kernel) # NOTE: I should find ball first # Find contour of white object in field whiteObjectContours = cv2.findContours(whiteObjectInFieldMask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)[1] # NOTE: All of this, I will do again if this idea is work # Loop over rectangle # Get bounding rect x, y, w, h # Filtered size that doesn't make sense # Get new four point # Expand bounding box from fourpoints # Cut roi from the four points and store # Append to imageList # Hog predict roi of image list # Get score vector # Find goal with old approach # Create score from result's old approach # Calculate score # Find mean between two score # Ranking if len(whiteObjectContours) != 0: for cnt in whiteObjectContours: # Get bounding box (x,y,width,height) x, y, w, h = cv2.boundingRect(cnt) if w < 10 or h < 10 and h > w: continue # append to position list centerX = x + w / 2 centerY = y + h / 2 positionList.append((centerX, centerY)) # Get top and left point topLeft = (x, y) bottomRight = (x + w, y + h) # Expand area newTopLeft_X, newTopLeft_Y, newBottomRight_X, newBottomRight_Y = expandBoundingBox( topLeft, bottomRight, imageWidth, imageHeight) # Get new width and height newWidth = newBottomRight_X - newTopLeft_X newHeight = newBottomRight_Y - newTopLeft_Y # Calculate new rectangle contours fourPointList = generateFourpoints( (newTopLeft_X, newTopLeft_Y), newWidth, newHeight) # Change to form of contours fourPointContours = np.array(fourPointList).reshape(-1, 1, 2) # Append new fourpoints to the list self.boundingBoxList.append(fourPointContours) # Crop image from gray scale image imgROI = grayImage[newTopLeft_Y:newBottomRight_Y, newTopLeft_X:newBottomRight_X].copy() # Resize to 40, 40 imgROI = cv2.resize(imgROI, (40, 40)) # Append to image list imageROIList.append(imgROI) if len(imageROIList) != 0: # Get feature matrix dim = (nSample, nFeatureVectors) featureMatrix = self.hog.extract(imageROIList) # Get probability score of each sample probabilityScoreVector = self.model.predict_proba( featureMatrix)[:, 1] # old approach !!! pointGoalList = findGoal(ransacContours, marker) # Create other score vector pointPolygonScoreVector = np.zeros( probabilityScoreVector.shape, dtype=float) if pointGoalList is not None: self.pointGoalList = pointGoalList # Loop point first for p in pointGoalList: # Loop each bounding rectangle for idx, cnt in enumerate(self.boundingBoxList): # Get width and height of rectangle contours width = cnt[2, 0, 0] - cnt[1, 0, 0] height = cnt[0, 0, 1] - cnt[1, 0, 1] # If there have any point that inside rectangle score = cv2.pointPolygonTest(cnt, p, True) if score > 0: # Calculate normalize score (0-1) and plus score to that bounding box score /= (min(width, height) / 2.0) pointPolygonScoreVector[idx] += score # print "Score from model of each candidate : {}".format( probabilityScoreVector ) # print "Score from check point : {}".format( pointPolygonScoreVector ) # find mean of score # meanScoreVector = probabilityScoreVector + pointPolygonScoreVector / 2 for idx, pos in enumerate(positionList): scoreDict[pos] = (0.8 * probabilityScoreVector[idx]) + ( 0.2 * pointPolygonScoreVector[idx]) # ranking score rankScore = sorted(scoreDict.items(), key=operator.itemgetter(1), reverse=True) nameList = list() pos2DList = list() errorList = list() confidenceList = list() # print rankScore if rankScore[0][1] > 0.5: for i in xrange(len(rankScore)): nameList.append('goal_{}'.format(i + 1)) pos2DList.append( Point32(rankScore[i][0][0], rankScore[i][0][1], 0.0)) errorX, errorY = self.calculateError( imageWidth, imageHeight, rankScore[i][0][0], rankScore[i][0][1]) errorList.append(Point32(errorX, errorY, 0.0)) confidenceList.append(rankScore[i][1]) msg = self.createVisionMsg(nameList, pos2DList, errorList, confidenceList, imageWidth, imageHeight) else: msg = self.createVisionMsg(list(), list(), list(), list(), imageWidth, imageHeight) else: msg = self.createVisionMsg(list(), list(), list(), list(), imageWidth, imageHeight) else: msg = self.createVisionMsg(list(), list(), list(), list(), imageWidth, imageHeight) # Initial msg # msg = visionMsg() # msg.header.stamp = rospy.Time.now() # msg.object_name = list() # msg.pos2D = list() # msg.object_error = list() # Later... # msg.object_confidence = list() # Later... # msg.imgH = imageHeight # msg.imgW = imageWidth # # Send goal first # for i, point in enumerate( filteredPosition ): # msg.object_name.append( 'goal_{}'.format( i+1 ) ) # msg.pos2D.append( Point32( point[0],point[1],0 ) ) # Return msg # msg.header.stamp = rospy.Time.now() # msg.object_name = [ 'ball', ] # msg.pos2D = [ Point32() ] # msg.imgH = imageHeight # msg.imgW = imageWidth # msg.object_error = [ Point32() ] # msg.object_confidence = [ 0.0 ] rospy.loginfo("Time usage : {}".format(time.time() - startTime)) return msg
def kinematicCalculation(self, objMsg, js, rconfig=None): stepSize = 20 img = cvtImageMessageToCVImage(objMsg) gray = img[:, :, 0:1] gray = np.repeat(gray, 3, axis=2) color_map = img[:, :, 1].copy() fieldContour, fieldMask = findBoundary(color_map, 2) points = [] for p in fieldContour[1:-1:stepSize, 0, :]: if p[1] == 0: continue points.append((p[0], p[1])) cv2.circle(gray, (p[0], p[1]), 4, (0, 0, 255), -1) cv2.imshow('img', gray) k = cv2.waitKey(1) if k == ord('w') and len(points) != 0: print "capture {} w".format(len(self.__imgPoints) + 1) panTiltPos = np.array(js.position, dtype=np.float64) self.__imgPoints.append(np.vstack(points)) self.__panTiltPoses.append(panTiltPos) self.__lineCoef.append([1.0, 0.0, -2.0]) elif k == ord('a') and len(points) != 0: print "capture {} a".format(len(self.__imgPoints) + 1) panTiltPos = np.array(js.position, dtype=np.float64) self.__imgPoints.append(np.vstack(points)) self.__panTiltPoses.append(panTiltPos) self.__lineCoef.append([0.0, 1.0, -2.0]) elif k == ord('d') and len(points) != 0: print "capture {} d".format(len(self.__imgPoints) + 1) panTiltPos = np.array(js.position, dtype=np.float64) self.__imgPoints.append(np.vstack(points)) self.__panTiltPoses.append(panTiltPos) self.__lineCoef.append([0.0, 1.0, 2.0]) elif k == ord('q'): print "Delete last instance." self.__imgPoints.pop(-1) self.__panTiltPoses.pop(-1) self.__lineCoef.pop(-1) elif k == ord('s') and len(self.__imgPoints) != 0: fn = '/tmp/line.npz' print "Save ... to {}".format(fn) np.savez(fn, imagePoints=self.__imgPoints, pantilt=self.__panTiltPoses, lineCoef=self.__lineCoef) return Empty()
def ImageProcessingFunction( self, img, header ): ''' ImageProcessingFunction USE WATERSHED BEFORE. ''' # get image property imageHeight = img.shape[ 0 ] imageWidth = img.shape[ 1 ] # get gray scale image imageGray = img[ :, :, 0 ].copy() # get marker (on channel 1) marker = img[ :, :, 1 ] # get field boundary fieldContour, fieldMask = findBoundary( marker, 2, flip = False ) self.contourFieldVis = fieldContour # # get white object and predict ball # # whiteObject = np.zeros( marker.shape, dtype = np.uint8 ) # whiteObject[ marker == 5 ] = 1 # # get white object only the field # whiteObjectInField = whiteObject * fieldMask # whiteObjectInField *= 255 # Get magenta object ballMagentaObject = np.zeros( marker.shape, dtype = np.uint8 ) ballMagentaObject[ marker == MagentaColorID ] = 1 ballMagentaObjectInField = fieldMask * ballMagentaObject ballMagentaObjectInField *= 255 # Get orange color object ballOrangeObject = np.zeros( marker.shape, dtype = np.uint8 ) ballOrangeObject[ marker == OrangeColorID ] = 1 ballOrangeObjectInField = fieldMask * ballOrangeObject ballOrangeObjectInField *= 255 # Get contour of orange and magenta ballMagentaObjectContour = cv2.findContours( ballMagentaObjectInField, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE )[1] ballOrangeObjectContour = cv2.findContours( ballOrangeObjectInField, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE )[1] objectNameList = list() pos2DList = list() objectErrorList = list() objectConfidenceList = list() if len( ballMagentaObjectContour ) != 0: x,y,w,h = cv2.boundingRect( ballMagentaObjectContour[ 0 ] ) objectNameList.append( 'ball_magenta' ) pos2DList.append( Point32( x = x + (w/2), y = y + (h/2), z = 0 ) ) errorX, errorY = self.calculateError( imageWidth, imageHeight, x + (w/2), y + (h/2) ) objectErrorList.append( Point32( x = errorX, y = errorY, z = 0.0 ) ) objectConfidenceList.append( 1.0 ) if len( ballOrangeObjectContour ) != 0: x, y, w, h = cv2.boundingRect( ballOrangeObjectContour[ 0 ] ) objectNameList.append( 'ball_orange' ) pos2DList.append( Point32( x = x + (w/2), y = y + (h/2), z = 0 ) ) errorX, errorY = self.calculateError( imageWidth, imageHeight, x + (w/2), y + (h/2) ) objectErrorList.append( Point32( x = errorX, y = errorY, z = 0.0 ) ) objectConfidenceList.append( 1.0 ) msg = self.createVisionMsg( objectNameList, pos2DList, objectErrorList, objectConfidenceList, imageWidth, imageHeight ) time.sleep(0.25) return msg
def main(): # initialize camera cap = cv2.VideoCapture( '/home/neverholiday/work/color_calibrator/video/field3.avi') # # initialize classifier # modelPathStr = ROS_WORKSPACE + '/src/hanuman_user/model/data_haar111217_2.xml' classifier = cv2.CascadeClassifier(modelPathStr) # get config ini pathConfig = 'color.ini' # get color dictionary colorDict = readConfig(pathConfig) while True: # capture ret, frame = cap.read() if ret: # change to gray image grayImage = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # smoothen image blurImage = cv2.blur(frame, (5, 5)) # marker marker = colorSegmentation(blurImage, colorDict) # get field boundary fieldBoundary, fieldMask = findBoundary(marker, 1, flip=False) # ged rid off outside boundary of field fieldImage = cv2.bitwise_and(grayImage, grayImage, mask=fieldMask) # get ball footballs = classifier.detectMultiScale(fieldImage, 1.1, 5) for (x, y, w, h) in footballs: cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2) #print np.var( fieldBoundary[ :, 1 ]) # throw marker to water shed #markerWaterShed = waterShed( frame, marker ) # draw contours cv2.drawContours(frame, [fieldBoundary], 0, (0, 0, 255), 3) # get render image colorImage = renderImage(marker, colorDict) cv2.imshow("image", frame) cv2.imshow("segmentation", colorImage) cv2.imshow("mask", fieldMask) k = cv2.waitKey(1) if k == ord('q'): break else: cap.set(cv2.CAP_PROP_POS_FRAMES, 0) cap.release() cv2.destroyAllWindows()
print "Used time {}".format( time.time() - startTime ) flag = False startTime = time.time() ret, img = cap.read() print "Used time {}".format( time.time() - startTime ) # filering blurImage = cv2.GaussianBlur( img, ( 5, 5 ), 0 ) # get marker marker = colorSegmentation( blurImage, colorDef ) # get field fieldContour, fieldMask = findBoundary( marker, 2, flip = False ) # get regressor list regressorList = findLinearEqOfFieldBoundary( fieldContour ) for regressor in regressorList: X = np.arange( regressor[ 2 ], regressor[ 3 ] ) Y = ( regressor[ 0 ] * X ) + regressor[ 1 ] floorY = np.floor( Y ).astype( X.dtype ) contour = np.vstack( ( X, floorY ) ).transpose() contour = contour.reshape( -1, 1, 2 ) cv2.drawContours( img, [ contour ], 0, ( 0, 0, 255 ), 3 )
def ImageProcessingFunction(self, img, header): stepSize = 20 imgH, imgW = img.shape[:2] color_map = img[:, :, 1].copy() fieldContour, fieldMask = findBoundary(color_map, 2) boundaryLine = findLinearEqOfFieldBoundary(fieldContour) ransacContours = [[0, imgH]] for m, c, x0, xf in boundaryLine: ransacContours.append([x0, m * x0 + c]) ransacContours.append([xf, m * xf + c]) ransacContours.append([imgW, imgH]) ransacContours = np.vstack(ransacContours).astype(int).reshape( -1, 1, 2) pointClound = findChangeOfColor(color_map, self.whiteID, self.greenID, mask=fieldMask, step=stepSize) points = [] splitIndexes = [] for i, scanline in enumerate(pointClound): for x, y in scanline: points.append(point2D(x=x, y=y)) splitIndexes.append(len(points)) for p in fieldContour[1:-1:stepSize, 0, :]: if p[1] < 10: continue points.append(point2D(x=p[0], y=p[1])) newFieldContour = findNewLineFromRansac(fieldContour, 640, 480) # goalList = findGoal( newFieldContour, color_map, goalColorID = 2 ) goalList = [] landmarkName = [] landmarkPose = [] landmarkConfidence = [] if len(boundaryLine) == 2: intersect_x = boundaryLine[0][3] intersect_y = boundaryLine[0][0] * intersect_x + boundaryLine[0][1] landmarkName.append('field_corner') landmarkPose.append(point2D(x=intersect_x, y=intersect_y)) landmarkConfidence.append(0.85) self.whiteObjectContours = self.getWhiteObjectContour( color_map, ransacContours) canExtract = self.predictor.extractFeature( img[:, :, 0], self.whiteObjectContours, objectPointLocation="bottom") if canExtract: self.predictor.predict() goalList = self.predictor.getGoal() foundBall = self.predictor.getBestRegion() if foundBall: landmarkName.append('ball') # get bounding box object not only position bestBounding = self.predictor.getBestBoundingBox() # get another point of object botX, botY = bestBounding.bottom landmarkPose.append(point2D(botX, botY)) landmarkConfidence.append( bestBounding.footballProbabilityScore) for goalObj in goalList: landmarkName.append('goal') botX, botY = goalObj.bottom landmarkPose.append(point2D(botX, botY)) landmarkConfidence.append(goalObj.goalProbabilityScore) msg = localizationMsg() msg.pointClound.num_scanline = 8 msg.pointClound.min_range = 0 msg.pointClound.max_range = 2**16 - 1 msg.pointClound.range = [] msg.pointClound.points = points msg.pointClound.splitting_index = splitIndexes msg.header = header msg.landmark.names = landmarkName msg.landmark.pose = landmarkPose msg.landmark.confidences = landmarkConfidence return msg
def ImageProcessingFunction(self, img, header, pan_tilt=None): startTime = time.time() objNameList = list() pos2DList = list() errorList = list() confidenceList = list() imgH, imgW = img.shape[:2] marker = img[:, :, 1] gray = img[:, :, 0] t0 = time.time() fieldContour, fieldMask = findBoundary(marker, self.greenID) coeff = findLinearEqOfFieldBoundary(fieldContour) ransacContours = [[0, imgH]] for m, c, x0, xf in coeff: ransacContours.append([x0, m * x0 + c]) ransacContours.append([xf, m * xf + c]) ransacContours.append([imgW, imgH]) ransacContours = np.vstack(ransacContours).astype(int).reshape( -1, 1, 2) t1 = time.time() pointClound = findChangeOfColor(marker, self.whiteID, self.greenID, mask=fieldMask, step=20) t2 = time.time() for scanline in pointClound: for x, y in scanline: self.addObject(x, y, 'point', 1.0, (imgH, imgW), objNameList, pos2DList, confidenceList, errorList) for p in fieldContour[1:-1:20, 0, :]: if p[1] < 10: continue self.addObject(p[0], p[1], 'field', 1.0, (imgH, imgW), objNameList, pos2DList, confidenceList, errorList) t3 = time.time() if len(coeff) > 1: # find y intersect xIntersect = coeff[0][3] m = coeff[0][0] c = coeff[0][1] yIntersect = (m * xIntersect) + c self.addObject(xIntersect, yIntersect, 'field_corner', 1.0, (imgW, imgH), objNameList, pos2DList, confidenceList, errorList) self.whiteObjectContours = self.getWhiteObjectContour( marker, ransacContours) t4 = time.time() self.kickCandidate = self.findKickCandidate(marker == 5) self.addObject(self.kickCandidate[0], self.kickCandidate[1], 'kicking_side', 1.0, (imgW, imgH), objNameList, pos2DList, confidenceList, errorList) magentaCnt, cyanCnt = self.getMagentaAndCyanContour(marker, fieldMask) if len(magentaCnt) > 0: largestMagenta = magentaCnt[-1] magentaArea = cv2.contourArea(largestMagenta) largestMagenta = cv2.moments(largestMagenta) else: magentaArea = 0 if len(cyanCnt) > 0: largestCyan = cyanCnt[-1] cyanArea = cv2.contourArea(largestCyan) largestCyan = cv2.moments(largestCyan) else: cyanArea = 0 if cyanArea + magentaArea > 0: largestArea = max(cyanArea, magentaArea) cyanCfd = cyanArea / largestArea magentaCfd = magentaArea / largestArea if cyanArea > 0 and largestCyan['m00'] != 0: x = largestCyan['m10'] / largestCyan['m00'] y = largestCyan['m01'] / largestCyan['m00'] self.addObject(x, y, 'cyan', cyanArea, (imgW, imgH), objNameList, pos2DList, confidenceList, errorList) if magentaArea > 0 and largestMagenta['m00'] != 0: x = largestMagenta['m10'] / largestMagenta['m00'] y = largestMagenta['m01'] / largestMagenta['m00'] self.addObject(x, y, 'magenta', magentaArea, (imgW, imgH), objNameList, pos2DList, confidenceList, errorList) t5 = time.time() canExtract = self.predictor.extractFeature( gray, self.whiteObjectContours, objectPointLocation="bottom") self.visBBList = self.predictor.boundingBoxListObject.boundingBoxList numCandidate = self.predictor.boundingBoxListObject.getNumberCandidate( ) t6 = time.time() predictTimeStart1 = 0 predictTimeEnd1 = 0 predictTimeStart2 = 0 predictTimeEnd2 = 0 if canExtract: # predictTimeStart1 = time.time() # self.predictor.predict() # predictTimeEnd1 = time.time() predictTimeStart2 = time.time() self.predictor.predict() predictTimeEnd2 = time.time() # self.predictor.printScore() goalList = self.predictor.getGoal() foundBall = self.predictor.getBestRegion() if foundBall: # get bounding box object not only position bestBounding = self.predictor.getBestBoundingBox() # get another point of object botX, botY = bestBounding.bottom centerX, centerY = bestBounding.center print bestBounding.footballProbabilityScore self.addObject(botX, botY, 'ball', bestBounding.footballProbabilityScore, (imgW, imgH), objNameList, pos2DList, confidenceList, errorList, error=(centerX, centerY)) for goalObj in goalList: botX, botY = goalObj.bottom self.addObject(botX, botY, 'goal', goalObj.goalProbabilityScore, (imgW, imgH), objNameList, pos2DList, confidenceList, errorList) t7 = time.time() msg = self.createVisionMsg(objNameList, pos2DList, errorList, confidenceList, imgW, imgH) t8 = time.time() rospy.logdebug("Time usage : {}".format(time.time() - startTime)) rospy.logdebug(" Time ransac : {}".format(t1 - t0)) rospy.logdebug(" Time point clound : {}".format(t2 - t1)) rospy.logdebug(" Time append pc : {}".format(t3 - t2)) rospy.logdebug(" Time white cnt : {}".format(t4 - t3)) rospy.logdebug(" Time kick cand : {}".format(t5 - t4)) rospy.logdebug(" Time ext feat : {}".format(t6 - t5)) rospy.logdebug(" Number of candidate : {}".format(numCandidate)) rospy.logdebug(" Time predict : {}".format(t7 - t6)) rospy.logdebug( " Time original predict only : {}".format(predictTimeEnd1 - predictTimeStart1)) rospy.logdebug( " Time eiei predict only : {}".format(predictTimeEnd2 - predictTimeStart2)) rospy.logdebug(" Time create msg : {}".format(t8 - t7)) return msg
def ImageProcessingFunction(self, img, header): startTime = time.time() objNameList = list() pos2DList = list() errorList = list() confidenceList = list() imageWidth = img.shape[1] imageHeight = img.shape[0] blurImage = cv2.GaussianBlur(img, (5, 5), 0) hsvImage = cv2.cvtColor(blurImage, cv2.COLOR_BGR2HSV) marker = colorSegmentation(blurImage, self.colorDefList) marker = cv2.watershed(hsvImage, marker) fieldContour, fieldMask = findBoundary(marker, 2) ransac = findNewLineFromRansac(fieldContour, imageWidth, imageHeight) if len(ransac) > 0: ransacContours, coeff = ransac[0], ransac[1] else: ransacContours, coeff = fieldContour, [] if len(coeff) > 1: # find y intersect xIntersect = coeff[0][3] m = coeff[0][0] c = coeff[0][1] yIntersect = (m * xIntersect) + c intersectPoint = Point32(x=xIntersect, y=yIntersect, z=0.0) errorX, errorY = self.calculateError(imageWidth, imageHeight, xIntersect, yIntersect) errorIntersectPoint = Point32(x=errorX, y=errorY, z=0.0) objNameList.append('field_corner') pos2DList.append(intersectPoint) errorList.append(errorIntersectPoint) confidenceList.append(1.0) newFieldMask = np.zeros(marker.shape, dtype=np.uint8) cv2.drawContours(newFieldMask, [ransacContours], 0, 1, -1) whiteObjectMask = np.zeros(marker.shape, dtype=np.uint8) whiteObjectMask[marker == 5] = 1 whiteObjectInFieldMask = whiteObjectMask * newFieldMask * 255 kernel = np.ones((5, 5), dtype=np.uint8) whiteObjectInFieldMask = cv2.morphologyEx(whiteObjectInFieldMask, cv2.MORPH_OPEN, kernel) whiteObjectContours = cv2.findContours(whiteObjectInFieldMask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)[1] canExtract = self.predictor.extractFeature( img, whiteObjectContours, objectPointLocation="bottom") if canExtract: self.predictor.predict() goalList = self.predictor.getGoal() bestPosition = tuple(self.predictor.getBestRegion()) if len(bestPosition) != 0: objNameList.append('ball') # get bounding box object not only position bestBounding = self.predictor.getBestBoundingBox() # get another point of object centerX, centerY = bestBounding.calculateObjectPoint('center') pos2DList.append( Point32(bestPosition[0][0], bestPosition[0][1], 0.0)) # calculate error errorX, errorY = self.calculateError(imageWidth, imageHeight, centerX, centerY) errorList.append(Point32(errorX, errorY, 0.0)) confidenceList.append(bestPosition[1]) for goal in goalList: objNameList.append('goal') pos2DList.append(Point32(goal[0][0], goal[0][1], 0.0)) errorX, errorY = self.calculateError(imageWidth, imageHeight, goal[0][0], goal[0][1]) errorList.append(Point32(errorX, errorY, 0.0)) confidenceList.append(goal[1]) poles = findPole(marker, self.orangeID, self.blueID, self.yellowID, self.magentaID, mask=fieldMask) for n, center in poles.items(): for x, y in center: objNameList.append(n) pos2DList.append(Point32(x=x, y=y)) errorX, errorY = self.calculateError(imageWidth, imageHeight, x, y) errorList.append(Point32(x=errorX, y=errorY)) confidenceList.append(1.0) msg = self.createVisionMsg(objNameList, pos2DList, errorList, confidenceList, imageWidth, imageHeight) rospy.logdebug("Time usage : {}".format(time.time() - startTime)) return msg
def main( ): usage = "python %prog vidoeInputPath colorConfigPath" parser = optparse.OptionParser( usage = usage ) parser.add_option( '--colorGreen', dest = 'colorGreen', type = 'int', help = 'Color ID for green color.', default = 1 ) parser.add_option( '--colorWhite', dest = 'colorWhite', type = 'int', help = 'Color ID for white color.', default = 0 ) options, args = parser.parse_args( ) assert len( args ) == 2, "This program take exacly 1 argument ({} given).".format( len(args) ) videoPath = args[0] colorConfigPath = args[1] colorDef = readConfig( colorConfigPath ) cap = cv2.VideoCapture( videoPath ) #cap = cv2.VideoCapture( 1 ) frameIdx = 0 while True: ret, frame = cap.read( ) cap.set( cv2.CAP_PROP_POS_FRAMES, frameIdx ) # get image width and height imageWidth, imageHeight = frame.shape[ 1 ], frame.shape[ 0 ] if not ret: cap.set( cv2.CAP_PROP_POS_FRAME, 0 ) continue blurImage = cv2.GaussianBlur( frame, (5,5), 0 ) marker = colorSegmentation( blurImage, colorDef ) marker = waterShed( blurImage, marker ) fieldContour, fieldMask = findBoundary( marker, 2 ) #fieldBoundary = findLinearEqOfFieldBoundary( fieldContour ) # find new contour ransacContours = findNewLineFromRansac( fieldContour, imageWidth, imageHeight ) ransacContours1 = ransacContours.copy() ransacContours2 = ransacContours.copy() ransacContours3 = ransacContours.copy() ransacContours1[ :, :, 1 ] = ransacContours1[ :, :, 1 ] + 5 ransacContours2[ :, :, 1 ] = ransacContours2[ :, :, 1 ] + 10 ransacContours3[ :, :, 1 ] = ransacContours3[ :, :, 1 ] + 15 previousPoint1 = ransacContours1[ 1 ] previousPoint2 = ransacContours2[ 1 ] previousPoint3 = ransacContours3[ 1 ] changeList1 = list() changeList2 = list() changeList3 = list() # cut origin and final position for point in ransacContours1[ 1:-1 ]: if marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] != marker[ previousPoint1[ 0 ][ 1 ], previousPoint1[ 0 ][ 0 ] ]: if marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] == 2 or marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] == 5: #isChange = True changeList1.append( point ) previousPoint1 = point for point in ransacContours2[ 1:-1 ]: if marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] != marker[ previousPoint2[ 0 ][ 1 ], previousPoint2[ 0 ][ 0 ] ]: if marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] == 2 or marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] == 5: #isChange = True changeList2.append( point ) previousPoint2 = point for point in ransacContours3[ 1:-1 ]: if marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] != marker[ previousPoint3[ 0 ][ 1 ], previousPoint3[ 0 ][ 0 ] ]: if marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] == 2 or marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] == 5: #isChange = True changeList3.append( point ) previousPoint3 = point changeArray2 = np.array( changeList2 ).reshape( -1, 2 ) changeArray3 = np.array( changeList3 ).reshape( -1, 2 ) print "************** ChangeArray *****************" print changeArray2 print changeArray3 if len( changeArray2 ) != 0 and len( changeArray3 ) != 0: distanceMatrix = spatial.distance.cdist( changeArray2, changeArray3 ) minIdxAxis_0 = np.argmin( distanceMatrix, axis = 0 ) minIdxAxis_1 = np.argmin( distanceMatrix, axis = 1 ) print "************** Idx *****************" print minIdxAxis_0 print minIdxAxis_1 print "************** Distance *****************" print distanceMatrix # for originPoints in changeList1: # x = originPoints[ 0 ][ 0 ] # y = originPoints[ 0 ][ 1 ] # # cv2.circle( frame, ( x, y ), 3, ( 0, 0, 255 ), -1 ) # for originPoints in changeList2: # x = originPoints[ 0 ][ 0 ] # y = originPoints[ 0 ][ 1 ] # # cv2.circle( frame, ( x, y ), 3, ( 0, 0, 255 ), -1 ) # # for originPoints in changeList3: # x = originPoints[ 0 ][ 0 ] # y = originPoints[ 0 ][ 1 ] # # cv2.circle( frame, ( x, y ), 3, ( 0, 0, 255 ), -1 ) #print marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] == 2, marker[ point[ 0 ][ 1 ], point[ 0 ][ 0 ] ] == 5 goalPointList = list() for i, j in zip( minIdxAxis_0, minIdxAxis_1 ): if i == j and 4.5 < distanceMatrix[ i, j ] < 6.5: x1, y1 = changeList2[ i ][ 0 ][ 0 ], changeList2[ i ][ 0 ][ 1 ] x2, y2 = changeList3[ j ][ 0 ][ 0 ], changeList3[ j ][ 0 ][ 1 ] goalPointList.append( ( x1, y1 ) ) cv2.circle( frame, ( x1, y1 ), 3, ( 0, 255, 0 ), -1 ) cv2.circle( frame, ( x2, y2 ), 3, ( 0, 0, 255 ), -1 ) print goalPointList if len( goalPointList ) == 4: print "found goal on frame : {}".format( cap.get( cv2.CAP_PROP_POS_FRAMES ) ) distanceMatrix2 = spatial.distance.cdist( goalPointList, goalPointList ) # print distanceMatrix2 # print goalPointList # minIdxAxisGoal_0 = np.argmin( distanceMatrix2[ np.nonzero( distanceMatrix2 ) ], axis = 0 ) # minIdxAxisGoal_1 = np.argmin( distanceMatrix2[ np.nonzero( distanceMatrix2 ) ], axis = 1 ) # # print "idx : {}, {}".format( minIdxAxisGoal_0, minIdxAxisGoal_1 ) # waitTime = 1 # # for m, c, x0, xf in fieldBoundary: # # c += 5 # # y0 = ( m * x0 ) + c # yf = ( m * xf ) + c # # p_list.append( (int(x0), int(y0), int(xf), int(yf)) ) # # changeFG2BG = [] # changeBG2FG = [] # FG = [] # BG = [] # # for x0, y0, xf, yf in p_list: # print x0, y0, xf, yf # # cv2.line( frame, (x0,y0), (xf,yf), (0,255,255), 1 ) # cv2.circle( frame, (x0,y0), 5, (0,0,255), -1 ) # if xf < 0: # print p_list # print fieldBoundary # waitTime = 0 # changeFG2BG_, changeBG2FG_, FG_, BG_ = raycast_bresenham( x0, y0, xf, yf, marker, options.colorGreen, options.colorWhite ) # # changeFG2BG.extend( changeFG2BG_ ) # changeBG2FG.extend( changeBG2FG_ ) # FG.extend( FG_ ) # BG.extend( BG_ ) # for x,y in BG: # cv2.circle( frame, (x,y), 2, (0,255,0), -1 ) # # for x,y in FG: # cv2.circle( frame, (x,y), 2, (0,0,0), -1 ) # # for x,y in changeFG2BG: # cv2.circle( frame, (x,y), 3, (0,0,255), -1 ) # # for x,y in changeBG2FG: # cv2.circle( frame, (x,y), 3, (255,0,0), -1 ) cv2.drawContours( frame, [ ransacContours ], 0, ( 255, 0, 0 ), 1 ) renderImg = renderColor( marker, colorDef ) cv2.imshow( 'frame', frame ) cv2.imshow( 'rendered', renderImg ) k = cv2.waitKey( 1 ) if k == ord( 'q' ): break if k == ord( 'a' ): frameIdx -= 1 if k == ord( 'd' ): frameIdx += 1 cv2.destroyAllWindows( )
def ImageProcessingFunction( self, img, header ): startTime = time.time() # get image property imageWidth = img.shape[ 1 ] imageHeight = img.shape[ 0 ] # initial final position bestPosition = Point32() ballErrorList = Point32() ballConfidence = 0.0 # blur image and change to hsv format blurImage = cv2.GaussianBlur( img, ( 5, 5 ), 0 ) # segmentation and get marker marker = colorSegmentation( blurImage, self.colorDefList ) # get field boundery, green color ID is 2 fieldContour, fieldMask = findBoundary( marker, 2, flip = False ) fieldContourMiddle = fieldContour[ 1:-1 ].copy() # # create new mask from ransac # # get ransac result linePropertyAttribute = findLinearEqOfFieldBoundary( fieldContourMiddle ) # create list of poit pointList = list() for lineCoeff in linePropertyAttribute: # get linear coefficient x0 = lineCoeff[ 2 ] xf = lineCoeff[ 3 ] m = lineCoeff[ 0 ] c = lineCoeff[ 1 ] # calculate y from x x = np.arange( x0, xf, dtype = np.int64 ) y = np.int64( m * x ) + int( c ) contour = np.vstack( ( x, y ) ).transpose() countour = contour.reshape( -1, 1, 2 ) pointList.append( countour ) if len( pointList ) > 1: contour = np.vstack( pointList ) # print pointList[ 0 ].shape # print pointList[ 1 ].shape # print contour.shape else: contour = pointList[ 0 ] firstPoint = np.array( [ [ [ 0, img.shape[ 0 ] - 1 ] ] ] ) lastPoint = np.array( [ [ [ img.shape[ 1 ] - 1, img.shape[ 0 ] - 1 ] ] ] ) contour = np.concatenate( ( firstPoint, contour, lastPoint ) ) self.contourVis1 = fieldContour self.contourVis2 = contour newFieldMask = np.zeros( marker.shape, dtype = np.uint8 ) cv2.drawContours( newFieldMask, [ contour ], 0, 1, -1 ) # # find white contour in mask # # get white object from marker color of white ID is 5 whiteObject = np.zeros( marker.shape, dtype = np.uint8 ) whiteObject[ marker == 5 ] = 1 # get white object only the field #whiteObjectInField = whiteObject * fieldMask.astype( np.uint8 ) whiteObjectInField = whiteObject * newFieldMask whiteObjectInField *= 255 # find contour from white object in field whiteContours = cv2.findContours( whiteObjectInField, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE )[ 1 ] # extract feature and predict it extractStatus = self.predictor.extractFeature( img, whiteContours, objectPointLocation = 'bottom' ) # check extract status if extractStatus == True: # predict si wait a rai self.predictor.predict() bestPositionList = self.predictor.getBestRegion() if len( bestPositionList ) != 0: # convert to point32 bestPosition = Point32( bestPositionList[ 0 ], bestPositionList[ 1 ], 0.0 ) # get bounding box object not only position bestBounding = self.predictor.getBestBoundingBox() # get another point of object centerX, centerY = bestBounding.calculateObjectPoint( 'center' ) # calculate error errorX, errorY = self.calculateError( imageWidth, imageHeight, centerX, centerY ) ballErrorList = Point32( errorX, errorY, 0.0 ) # ball confidence ballConfidence = 1.0 # define vision message instance msg = visionMsg() # assign to message msg.object_name = [ 'ball' ] msg.pos2D = [ bestPosition ] msg.imgH = imageHeight msg.imgW = imageWidth msg.object_error = [ ballErrorList ] msg.object_confidence = [ ballConfidence ] msg.header.stamp = rospy.Time.now() rospy.loginfo( "Time usage : {}".format( time.time() - startTime ) ) return msg
if not stop: ret, frame = cap.read( ) img = frame.copy() if not ret: print "Camera not found ..." break # hsv = cv2.cvtColor( img, cv2.COLOR_BGR2HSV ) img = cv2.GaussianBlur( img, ( 5, 5 ), 0 ) colorMap = colorSegmentation( img, colorDefList )[:, : ] colorMap_watershed = waterShed( img, colorMap.copy() ).astype( np.uint8 ) fieldContour, fieldMask = findBoundary( colorMap_watershed, 1 ) pointClound = findChangeOfColor( colorMap_watershed, 8, 1, mask=fieldMask, step = 40 ) # pointCloound = findChangeOfColor( colorMap, 8, 1, mask = fieldMask ) renderedColorMap = renderColor( colorMap, colorDefList) renderedColorMap_watershed = renderColor( colorMap_watershed, colorDefList) cv2.drawContours( img, [ fieldContour ], 0, (0, 128, 255), 2 ) for scanLine in pointClound: for x,y in scanLine: cv2.circle(img,(x,y), 4, (0,0,0), -1) cv2.circle(img,(x,y), 3, (0,0,255), -1) # cv2.imshow( 'colorMap', colorMap_watershed ) # cv2.imshow( 'colorMap_watershed', renderedColorMap_watershed )
def ImageProcessingFunction(self, img, header): # get image property imageWidth = img.shape[1] imageHeight = img.shape[0] # # get field mask # fieldMask = getFieldMask( img, self.lower, self.upper ) # # convert image to gray scale # grayImage = cv2.cvtColor( img, cv2.COLOR_BGR2GRAY ) # # and with gray color # grayImage = cv2.bitwise_and( grayImage, grayImage, mask = fieldMask ) # # detect ball # footballPositions = self.classifier.detectMultiScale( grayImage, 1.1, 5 ) # # check ball # if len( footballPositions ) > 0: # x, y, w, h = footballPositions[ 0 ] # ballPosition = [ x+w/2, y+h/2 ] # isBallDetection = True # ballError = self.findBallError( ballPosition[ 0 ], ballPosition[ 1 ], imageWidth, imageHeight ) # else: # ballPosition = list() # isBallDetection = False # ballError = list() # get color map and grey color grayImage = img[:, :, 0] colorMap = img[:, :, 1] # get field boundary fieldBoundary, fieldMask = findBoundary(colorMap, 1, flip=False) # get rid off region outside field boundary # colorMap *= fieldMask marker = np.ones(grayImage.shape, dtype=np.uint8) marker[colorMap == 1] = 0 grayImage *= fieldMask # whiteContours = cv2.findContours( marker, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE )[ 1 ] # circularWhiteList = self.detectCircular( whiteContours ) #footballs = self.detectBall( circularWhiteList, grayImage ) footballs = self.classifier.detectMultiScale(grayImage, 1.1, 3) if len(footballs) > 0: x, y, h, w = footballs[0] ballPosition = [x + w / 2, y + h / 2] isBallDetection = True ballError = self.findBallError(ballPosition[0], ballPosition[1], imageWidth, imageHeight) else: ballPosition = list() isBallDetection = False ballError = list() # # find contour with marker # whiteConturs = cv2.findContours( marker, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE )[ 1 ] # # find circular white object # self.circularWhiteList = self.detectCircular( whiteConturs ) # # detect ball # footballs = self.detectBall( self.circularWhiteList, grayImage ) # print footballs # if len( footballs ) > 0: # ballPosition = footballs[ 0 ][ 0 : 2 ] # isBallDetection = True # ballError = self.findBallError( ballPosition[ 0 ], ballPosition[ 1 ], imageWidth, imageHeight ) # else: # ballPosition = list() # isBallDetection = False # ballError = list() msg = visionMsg() msg.ball = ballPosition msg.imgH = grayImage.shape[0] msg.imgW = grayImage.shape[1] msg.ball_error = ballError msg.ball_confidence = isBallDetection msg.header.stamp = rospy.Time.now() return msg