def ImageProcessingFunction(self, img, header): # get image property imageWidth = img.shape[1] imageHeight = img.shape[0] # convert to hsv hsvImage = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) maskImage = cv2.inRange(hsvImage, self.__lower, self.__upper) # find contour contours = cv2.findContours(maskImage, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)[1] if len(contours) != 0: areaList = [cv2.contourArea(contour) for contour in contours] maxArea = max(areaList) idxContour = areaList.index(maxArea) ballContour = contours[idxContour] try: # find image moments M = cv2.moments(ballContour) # cx = int( M['m10'] / M['m00'] ) # cy = int( M['m01'] / M['m00'] ) cx, cy = tuple(ballContour[ballContour[:, :, 1].argmax()][0]) ballPosition = [int(cx), int(cy)] # calculate error # NOTE : edit error y for switch sign for control motor errorX = (ballPosition[0] - imageWidth / 2.) / (imageWidth / 2.) errorY = (ballPosition[1] - imageHeight / 2.) / (imageHeight / 2.) ballError = [errorX, errorY] isDetectBall = True except ZeroDivisionError: ballPosition = [] ballError = list() isDetectBall = False else: ballPosition = [] ballError = list() isDetectBall = False msg = visionMsg() msg.ball = ballPosition msg.imgH = hsvImage.shape[0] msg.imgW = hsvImage.shape[1] msg.ball_error = ballError msg.ball_confidence = isDetectBall msg.header.stamp = rospy.Time.now() self.__previousPosition = ballPosition return msg
def createVisionMsg( self, objectNameList, pos2DList, objectErrorList, objectConfidenceList, imgWidth, imgHeight ): ''' createVisionMsg function ''' msg = visionMsg() msg.header.stamp = rospy.Time.now() msg.object_name = objectNameList msg.pos2D = pos2DList msg.object_error = objectErrorList msg.object_confidence = objectConfidenceList msg.imgH = imgHeight msg.imgW = imgWidth return msg
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 ): 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
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