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
