def threshold(self, img, color):
self.allCentroidList = []
self.allAreaList = []
self.allRadiusList = []
#params = self.getParams(color)
# Perform thresholding
mask = cv2.inRange(img, self.redParams['lo3'], self.redParams['hi3'])
kern = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
# return cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
thresImg1 = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kern)
# thresImg1 = cv2.dilate(mask, kern, iterations=1)
mask2 = cv2.inRange(img, self.redParams['lo4'], self.redParams['hi4'])
thresImg2 = cv2.dilate(mask2, kern, iterations=2)
# binImg = cv2.bitwise_or(thresImg1, thresImg2)
binImg = thresImg1
# return cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR)
# Find contours
scratchImg = binImg.copy()
scratchImgCol = cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR)
contours, hierachy = cv2.findContours(scratchImg, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
contours = filter(lambda c: cv2.contourArea(c) > self.minContourArea,
contours)
sorted(contours, key=cv2.contourArea,
reverse=True) # Sort by largest contour
# If centering, just find the center of largest contour
if self.comms.isCentering:
if len(contours) > 0:
largestContour = contours[0]
mu = cv2.moments(largestContour)
muArea = mu['m00']
self.comms.centroidToBump = (int(mu['m10'] / muArea),
int(mu['m01'] / muArea))
self.comms.rectArea = muArea
self.previousCentroid = self.comms.centroidToBump
self.previousArea = self.comms.rectArea
else:
self.comms.centroidToBump = self.previousCentroid
self.comms.rectArea = self.previousArea
else:
# Find hough circles
circles = cv2.HoughCircles(
binImg,
cv2.cv.CV_HOUGH_GRADIENT,
1,
minDist=30,
param1=self.houghParams[0],
param2=self.houghParams[1],
minRadius=self.circleParams['minRadius'],
maxRadius=self.circleParams['maxRadius'])
# Check if center of circles inside contours
if contours is not None:
for contour in contours:
mu = cv2.moments(contour)
muArea = mu['m00']
centroid = (mu['m10'] / muArea, mu['m01'] / muArea)
if circles is not None:
for circle in circles[0, :, :]:
circleCentroid = (circle[0], circle[1])
if abs((Utils.distBetweenPoints(
centroid, circleCentroid))) < circle[2]:
self.comms.foundBuoy = True
# Find new centroid by averaging the centroid and circle centroid
newCentroid = (
int(centroid[0] + circleCentroid[0]) / 2,
int(centroid[1] + circleCentroid[1]) / 2)
self.allCentroidList.append(newCentroid)
self.allAreaList.append(
cv2.contourArea(contour))
self.allRadiusList.append(circle[2])
# Draw circles
cv2.circle(scratchImgCol, newCentroid,
circle[2], (255, 255, 0), 2)
cv2.circle(scratchImgCol, newCentroid, 2,
(255, 0, 255), 3)
# Find the circle with the largest radius
if not len(self.allCentroidList) == 0:
maxIndex = self.allRadiusList.index(max(self.allRadiusList))
self.comms.centroidToBump = self.allCentroidList[maxIndex]
self.comms.rectArea = self.allAreaList[maxIndex]
self.previousCentroid = self.comms.centroidToBump
self.previousArea = self.comms.rectArea
self.comms.grad = self.getGradient()
else:
self.comms.centroidToBump = self.previousCentroid
self.comms.rectArea = self.previousArea
cv2.putText(scratchImgCol, "Ang: " + str(self.comms.grad), (30, 100),
cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
# Draw new centroid
cv2.circle(scratchImgCol, self.comms.centroidToBump, 3, (0, 255, 255),
2)
# rospy.loginfo("Area: {}".format(self.comms.rectArea)) # To put on the scratchImg
cv2.putText(scratchImgCol, "Area: " + str(self.comms.rectArea),
(30, 80), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
# How far centroid is off screen center
self.comms.deltaX = float(
(self.comms.centroidToBump[0] - vision.screen['width'] / 2) * 1.0 /
vision.screen['width'])
cv2.putText(scratchImgCol, "X " + str(self.comms.deltaX), (30, 30),
cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
self.comms.deltaY = float(
(self.comms.centroidToBump[1] - vision.screen['height'] / 2) *
1.0 / vision.screen['height'])
cv2.putText(scratchImgCol, "Y " + str(self.comms.deltaY), (30, 60),
cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
# Draw center rect
scratchImgCol = vision.drawCenterRect(scratchImgCol)
return scratchImgCol
def threshold(self, img, color):
self.allCentroidList = []
self.allAreaList = []
self.allRadiusList = []
#params = self.getParams(color)
# Perform thresholding
mask = cv2.inRange(img, self.redParams['lo3'], self.redParams['hi3'])
kern = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
# return cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
thresImg1 = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kern)
# thresImg1 = cv2.dilate(mask, kern, iterations=1)
mask2 = cv2.inRange(img, self.redParams['lo4'], self.redParams['hi4'])
thresImg2 = cv2.dilate(mask2, kern, iterations=2)
# binImg = cv2.bitwise_or(thresImg1, thresImg2)
binImg = thresImg1
# return cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR)
# Find contours
scratchImg = binImg.copy()
scratchImgCol = cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR)
contours, hierachy = cv2.findContours(scratchImg, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
contours = filter(lambda c: cv2.contourArea(c) > self.minContourArea, contours)
sorted(contours, key=cv2.contourArea, reverse=True) # Sort by largest contour
# If centering, just find the center of largest contour
if self.comms.isCentering:
if len(contours) > 0:
largestContour = contours[0]
mu = cv2.moments(largestContour)
muArea = mu['m00']
self.comms.centroidToBump = (int(mu['m10']/muArea), int(mu['m01']/muArea))
self.comms.rectArea = muArea
self.previousCentroid = self.comms.centroidToBump
self.previousArea = self.comms.rectArea
else:
self.comms.centroidToBump = self.previousCentroid
self.comms.rectArea = self.previousArea
else:
# Find hough circles
circles = cv2.HoughCircles(binImg, cv2.cv.CV_HOUGH_GRADIENT, 1,
minDist=30, param1=self.houghParams[0],
param2=self.houghParams[1],
minRadius = self.circleParams['minRadius'],
maxRadius = self.circleParams['maxRadius'])
# Check if center of circles inside contours
if contours is not None:
for contour in contours:
mu = cv2.moments(contour)
muArea = mu['m00']
centroid = (mu['m10']/muArea, mu['m01']/muArea)
if circles is not None:
for circle in circles[0,:,:]:
circleCentroid = (circle[0], circle[1])
if abs((Utils.distBetweenPoints(centroid,circleCentroid))) < circle[2]:
self.comms.foundBuoy = True
# Find new centroid by averaging the centroid and circle centroid
newCentroid =(int(centroid[0]+circleCentroid[0])/2,
int(centroid[1]+circleCentroid[1])/2)
self.allCentroidList.append(newCentroid)
self.allAreaList.append(cv2.contourArea(contour))
self.allRadiusList.append(circle[2])
# Draw circles
cv2.circle(scratchImgCol, newCentroid, circle[2], (255, 255, 0), 2)
cv2.circle(scratchImgCol, newCentroid, 2, (255, 0, 255), 3)
# Find the circle with the largest radius
if not len(self.allCentroidList) == 0:
maxIndex = self.allRadiusList.index(max(self.allRadiusList))
self.comms.centroidToBump = self.allCentroidList[maxIndex]
self.comms.rectArea = self.allAreaList[maxIndex]
self.previousCentroid = self.comms.centroidToBump
self.previousArea = self.comms.rectArea
self.comms.grad = self.getGradient()
else:
self.comms.centroidToBump = self.previousCentroid
self.comms.rectArea = self.previousArea
cv2.putText(scratchImgCol, "Ang: " + str(self.comms.grad), (30, 100),
cv2.FONT_HERSHEY_PLAIN, 1, (204,204,204))
# Draw new centroid
cv2.circle(scratchImgCol, self.comms.centroidToBump, 3, (0, 255, 255), 2)
# rospy.loginfo("Area: {}".format(self.comms.rectArea)) # To put on the scratchImg
cv2.putText(scratchImgCol, "Area: " + str(self.comms.rectArea), (30, 80),
cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
# How far centroid is off screen center
self.comms.deltaX = float((self.comms.centroidToBump[0] - vision.screen['width']/2)*1.0/
vision.screen['width'])
cv2.putText(scratchImgCol, "X " + str(self.comms.deltaX), (30,30),
cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
self.comms.deltaY = float((self.comms.centroidToBump[1] - vision.screen['height']/2)*1.0/
vision.screen['height'])
cv2.putText(scratchImgCol, "Y " + str(self.comms.deltaY), (30,60),
cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
# Draw center rect
scratchImgCol = vision.drawCenterRect(scratchImgCol)
return scratchImgCol
def gotFrame(self, img):
allCentroidList = []
allRadiusList = []
img = cv2.resize(img, (640, 480))
# img = self.illumMask(img)
# img = self.whiteBal(img)
# img = self.illuminanceRemoval(img)
# hsvImg = self.toHSV(img)
# enhancedImg = self.enhance(hsvImg)
# return cv2.cvtColor(enhancedImg, cv2.COLOR_HSV2BGR)
labImg = cv2.cvtColor(img, cv2.COLOR_BGR2LAB)
channels = cv2.split(labImg)
# return cv2.cvtColor(channels[2], cv2.COLOR_GRAY2BGR)
# Threshold out something
binImg = self.morphology(
cv2.inRange(labImg, self.greenParams['lo'],
self.greenParams['hi']))
kern = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
binImg = cv2.morphologyEx(binImg, cv2.MORPH_OPEN, kern)
# return cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR)
# binImg = self.morphology(cv2.inRange(enhancedImg,
# self.greenParams['lo'], self.greenParams['hi']))
# erodeKern = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
# binImg = cv2.erode(binImg, erodeKern, iterations=2)
dilateKern = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
binImg = cv2.dilate(binImg, dilateKern, iterations=3)
# Find contours and fill them
for i in range(4):
binImgCopy = binImg.copy()
contours, hierarchy = cv2.findContours(binImgCopy, cv2.RETR_CCOMP,
cv2.CHAIN_APPROX_NONE)
cv2.drawContours(image=binImg,
contours=contours,
contourIdx=-1,
color=(255, 255, 255),
thickness=-1)
'''
Detect the board first
'''
# Detecting the board
# Find the largest contours and make sure its a square
scratchImgCol = cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR)
binImgCopy = binImg.copy()
contours, hierarchy = cv2.findContours(binImgCopy, cv2.RETR_CCOMP,
cv2.CHAIN_APPROX_NONE)
contours = filter(lambda c: cv2.contourArea(c) > self.minContourArea,
contours)
if len(contours) == 0:
return scratchImgCol
self.comms.foundSomething = True
sorted(contours, key=cv2.contourArea, reverse=True)
largestContour = contours[0]
rect = cv2.minAreaRect(largestContour)
((center_x, center_y), (width, height),
angle) = cv2.minAreaRect(largestContour)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
cv2.drawContours(scratchImgCol, [box], 0, (0, 0, 255), 2)
# Find centroid of rect returned
mu = cv2.moments(largestContour)
muArea = mu['m00']
tempBoardCentroid = (int(mu['m10'] / muArea), int(mu['m01'] / muArea))
tempBoardArea = muArea
# self.comms.skew = mu['m30']/(pow(mu['02'],1.5))
self.comms.boardCentroid = tempBoardCentroid
self.comms.boardArea = tempBoardArea
# Dist where centroid of board is off
self.comms.boardDeltaX = float(
(self.comms.boardCentroid[0] - vision.screen['width'] / 2) * 1.0 /
vision.screen['width'])
self.comms.boardDeltaY = float(
(self.comms.boardCentroid[1] - vision.screen['height'] / 2) * 1.0 /
vision.screen['height'])
cv2.putText(scratchImgCol, "Board Area: " + str(self.comms.boardArea),
(410, 30), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
cv2.circle(scratchImgCol, self.comms.boardCentroid, 2, (0, 0, 255), 2)
cv2.putText(scratchImgCol, "Board X: " + str(self.comms.boardDeltaX),
(410, 60), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
cv2.putText(scratchImgCol, "Board Y: " + str(self.comms.boardDeltaY),
(410, 80), cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
binImgCopy = binImg.copy()
contours, hierarchy = cv2.findContours(binImgCopy, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
self.comms.foundCircles = True
if not self.comms.isCenteringState:
contours = filter(lambda c: cv2.contourArea(c) < 10000, contours)
for contour in contours:
# if cv2.contourArea(contour) < 10000 and cv2.contourArea(contour) > self.minContourArea:
if cv2.contourArea(contour) > self.minContourArea:
(cx, cy), radius = cv2.minEnclosingCircle(contour)
cv2.drawContours(scratchImgCol, [contour], 0, (255, 0, 255), 2)
cv2.circle(scratchImgCol, (int(cx), int(cy)), int(radius),
(255, 255, 0), 2)
cv2.circle(scratchImgCol, (int(cx), int(cy)), 1, (255, 0, 255),
2)
allCentroidList.append((cx, cy, radius))
if contours is None:
self.comms.foundCount = self.comms.foundCount + 1
return scratchImgCol
sorted(allCentroidList, key=lambda centroid: centroid[2], reverse=True)
'''
Detect the circles
'''
'''
# Find Hough circles - used to be edges
scratchImg = binImg.copy()
circles = cv2.HoughCircles(scratchImg, cv2.cv.CV_HOUGH_GRADIENT, 1,
minDist=10, param1=self.houghParams[0],
param2=self.houghParams[1],
minRadius = self.circleParams['minRadius'],
maxRadius = self.circleParams['maxRadius'])
if circles is None:
self.comms.foundCount = self.comms.foundCount + 1
return scratchImgCol
self.comms.foundCircles = True
circlesSorted = np.array(sorted(circles[0], key=lambda x:x[2], reverse=True))
for circle in circlesSorted:
circleCentroid = (circle[0], circle[1])
# Only find circles within the bounding rect
if self.isInRect(circleCentroid, (center_x, center_y), (width, height)):
allCentroidList.append(circleCentroid)
allRadiusList.append(circle[2])
# Draw Circles
cv2.circle(scratchImgCol, circleCentroid, circle[2], (255, 255, 0), 2)
cv2.circle(scratchImgCol, circleCentroid, 2, (255, 0, 255), 3)
'''
# Centroid resetted
if self.comms.centroidToShoot is None:
'''
# Pick the largest circle
if self.comms.numShoot == 0 or len(circles) < 2:
self.comms.centroidToShoot = (circlesSorted[0][0], circlesSorted[0][1])
self.comms.radius = circlesSorted[0][2]
elif self.comms.numShoot == 1:
self.comms.centroidToShoot = (circlesSorted[1][0], circlesSorted[1][1])
self.comms.radius = circlesSorted[1][2]
'''
if self.comms.numShoot == 0:
self.comms.centroidToShoot = (int(allCentroidList[0][0]),
int(allCentroidList[0][1]))
self.comms.radius = allCentroidList[0][2]
elif self.comms.numShoot == 1:
self.comms.centroidToShoot = (int(allCentroidList[1][0]),
int(allCentroidList[1][1]))
self.comms.radius = allCentroidList[1][2]
else:
# Find the centroid closest to the previous
if len(allCentroidList) != 0:
for centroid in allCentroidList:
distDiff = []
distDiff.append(
Utils.distBetweenPoints(self.previousCentroid,
(centroid[0], centroid[1])))
minIndex = distDiff.index(min(distDiff))
self.comms.centroidToShoot = (
int(allCentroidList[minIndex][0]),
int(allCentroidList[minIndex][1]))
self.comms.radius = allCentroidList[minIndex][2]
# self.comms.radius = allRadiusList[minIndex]
else:
# If not then just use the previous one
self.comms.centroidToShoot = self.previousCentroid
self.comms.radius = self.previousRadius
# Draw centroid to shoot
self.previousCentroid = self.comms.centroidToShoot
self.previousRadius = self.comms.radius
cv2.circle(scratchImgCol, self.comms.centroidToShoot, 3, (0, 255, 255),
2)
# How far the centroid is off the screen center
self.comms.deltaX = float(
(self.comms.centroidToShoot[0] - vision.screen['width'] / 2) *
1.0 / vision.screen['width'])
# Draw everything
cv2.putText(scratchImgCol, "X " + str(self.comms.deltaX), (30, 30),
cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 255))
self.comms.deltaY = float(
(self.comms.centroidToShoot[1] - vision.screen['height'] / 2) *
1.0 / vision.screen['height'])
cv2.putText(scratchImgCol, "Y " + str(self.comms.deltaY), (30, 60),
cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 255))
cv2.putText(scratchImgCol, "Rad " + str(self.comms.radius), (30, 85),
cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 255))
cv2.putText(scratchImgCol, "SHOOT TO KILL", (30, 430),
cv2.FONT_HERSHEY_PLAIN, 2, (255, 200, 255))
# Draw center of screen
scratchImgCol = vision.drawCenterRect(scratchImgCol)
return scratchImgCol
