def _removeSmallerContours(self, image):
cv2.rectangle(image, (0, 0), (640, 480), (0, 0, 0), thickness=1)
contours, _ = cv2.findContours(image.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
bestContour = max(contours, key=cv2.contourArea)
for contour in contours:
if not numpy.array_equal(contour, bestContour):
leftmostX = findLeftmostPoint(contour)[0]
topmostY = findTopmostPoint(contour)[1]
rightmostX = findRightmostPoint(contour)[0]
bottommostY = findBottommostPoint(contour)[1]
cv2.rectangle(image, (leftmostX, topmostY), (rightmostX, bottommostY), (255, 255, 255), thickness=-1)
contours, _ = cv2.findContours(image.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
bestContour = max(contours, key=cv2.contourArea)
for contour in contours:
if not numpy.array_equal(contour, bestContour):
leftmostX = findLeftmostPoint(contour)[0]
topmostY = findTopmostPoint(contour)[1]
rightmostX = findRightmostPoint(contour)[0]
bottommostY = findBottommostPoint(contour)[1]
cv2.rectangle(image, (leftmostX, topmostY), (rightmostX, bottommostY), (0, 0, 0), thickness=-1)
cv2.rectangle(image, (0, 0), (640, 480), (0, 0, 0), thickness=169)
return image
def findPositions(self):
positions = []
contours = self._findContours()
if len(contours) == 1:
# Three possible cases:
# 1. Both obstacles are visible, but they're too close to each other
# 2. One obstacle is hidden by the robot
# 3. One obstacle is exactly behind the other
# It is impossible to distinguish cases 2 and 3
contour = contours[0]
leftmostPoint = findLeftmostPoint(contour)
rightmostPoint = findRightmostPoint(contour)
leftmostX, leftmostZ = self.pointConvertor.convertPoint(leftmostPoint)
rightmostX, rightmostZ = self.pointConvertor.convertPoint(rightmostPoint)
if leftmostZ - self.OBSTACLE_DISTANCE_THRESHOLD <= rightmostZ <= leftmostZ + self.OBSTACLE_DISTANCE_THRESHOLD:
# Case 2/3
return [self._findObstaclePositionFromContour(contour)]
else:
# Case 1
return [(leftmostX, leftmostZ), (rightmostX, rightmostZ)]
elif len(contours) > 1:
for contour in contours:
try:
obstaclePosition = self._findObstaclePositionFromContour(contour)
except InvalidDistanceException, ValueError:
continue
positions.append(obstaclePosition)
def _findObstaclePositionFromContour(self, contour):
contourCenterPoint = findCenterPoint(contour)
contourLeftmostPoint = findLeftmostPoint(contour)
contourRightmostPoint = findRightmostPoint(contour)
firstPoint = findPointBetweenPoints(contourCenterPoint, contourLeftmostPoint, 1./2)
secondPoint = findPointBetweenPoints(contourCenterPoint, contourRightmostPoint, 1./2)
firstPosition = self.pointConvertor.convertPoint(firstPoint)
secondPosition = self.pointConvertor.convertPoint(secondPoint)
obstaclePosition = self._findObstacleCenter(firstPosition, secondPosition)
return obstaclePosition
def findPositionAndOrientation(self):
contour, color = self._findBestContour()
centerPoint = findCenterPoint(contour)
leftmostPoint = findLeftmostPoint(contour)
rightmostPoint = findRightmostPoint(contour)
# A factor of 1/2 will give the point halfway between the center and the leftmost/rightmost point
leftsidePoint = findPointBetweenPoints(centerPoint, leftmostPoint, 1./2)
rightsidePoint = findPointBetweenPoints(centerPoint, rightmostPoint, 1./2)
leftsidePosition = self.pointConvertor.convertPoint(leftsidePoint)
rightsidePosition = self.pointConvertor.convertPoint(rightsidePoint)
robotPosition, robotOrientation = self._findRobotCenterAndOrientation(leftsidePosition, rightsidePosition, color)
return robotPosition, robotOrientation
