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