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