class CvCamera(ICamera):
_buffer_size = 4
def __init__(self, camera_id: int) -> None:
self.image_display = CvImageDisplay()
self._video_capture = cv2.VideoCapture(camera_id)
if not self._video_capture.isOpened():
raise CameraDoesNotExistError(camera_id)
def __del__(self) -> None:
self._video_capture.release(
) # On Windows: Display "[ WARN:0] terminating async callback" in console
def take_picture(self) -> Image:
count = self._buffer_size
while count > 0:
self._video_capture.grab()
count -= 1
has_captured, cv_image = self._video_capture.read()
if not has_captured:
raise AcquisitionError()
self.image_display.display_debug_image('[OpenCvCamera] take_picture',
cv_image)
return Image(cv_image)
class CvObstacleFinder(IObstacleFinder):
def __init__(self) -> None:
self._aruco_dictionary = aruco.Dictionary_get(aruco.DICT_6X6_250)
self._obstacles_id = 0
self._detection_parameter = aruco.DetectorParameters_create()
self._obstacles: List[Rectangle] = []
self._image_display = CvImageDisplay()
def find(self, image: Image) -> List[Rectangle]:
image.process(self._process)
return self._obstacles
def _process(self, image: np.ndarray) -> None:
grey = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
_, threshold = cv2.threshold(grey, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
self._image_display.display_debug_image('[CvObstacleFinder] threshold', threshold)
corners, ids, _ = aruco.detectMarkers(threshold, self._aruco_dictionary, parameters=self._detection_parameter)
self._image_display.display_debug_aruco_markers('[CvObstacleFinder] markers', image, corners, ids)
self._obstacles.clear()
if ids is not None:
for i in range(ids.shape[0]):
if ids[i] == self._obstacles_id:
min_x = np.min(corners[i][0, :, 0])
max_x = np.max(corners[i][0, :, 0])
min_y = np.min(corners[i][0, :, 1])
max_y = np.max(corners[i][0, :, 1])
self._obstacles.append(Rectangle(min_x, min_y, max_x - min_x, max_y - min_y))
else:
raise ObstaclesCouldNotBeFound
def __init__(self) -> None:
self._aruco_dictionary = aruco.Dictionary_get(aruco.DICT_6X6_250)
self._top_left_id = 2
self._top_right_id = 3
self._bottom_left_id = 4
self._bottom_right_id = 5
self._ratio_distance_marker_size = 0.1
self._detection_parameter = aruco.DetectorParameters_create()
self._robot: Coord = None
self._orientation: Angle = None
self._image_display = CvImageDisplay()
def __init__(self, camera_matrix: np.ndarray,
distortion_coefficients: np.ndarray,
play_area_finder: IPlayAreaFinder, image: Image) -> None:
self._play_area_finder = play_area_finder
self._image_display = CvImageDisplay()
self._camera_matrix = camera_matrix
self._distortion_coefficients = distortion_coefficients
self._optimized_camera_matrix, self._region_of_interest = \
cv2.getOptimalNewCameraMatrix(self._camera_matrix, self._distortion_coefficients,
image.size, 1, image.size)
self._camera_matrix_inverse = np.linalg.inv(
self._optimized_camera_matrix)
self._focal_x: float = self._optimized_camera_matrix[0, 0]
self._focal_y: float = self._optimized_camera_matrix[1, 1]
self._compute_distances(image)
class CvPlayAreaFinder(IPlayAreaFinder):
def __init__(self) -> None:
self._play_area: Rectangle = None
self._image_display = CvImageDisplay()
def find(self, image: Image) -> Rectangle:
image.process(self._process)
return self._play_area
def _process(self, image: np.ndarray) -> None:
grey = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
self._image_display.display_debug_image('[OpenCvPlayAreaFinder] grey', grey)
_, threshold = cv2.threshold(grey, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
self._image_display.display_debug_image('[OpenCvPlayAreaFinder] threshold', threshold)
contours, _ = cv2.findContours(threshold, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
contours = [CvPlayAreaFinder._approximate_contour(c) for c in contours]
contours = [c for c in contours if CvPlayAreaFinder._does_contour_fit_area(c)]
if len(contours) == 0:
raise PlayAreaCouldNotBeFound
contour_table = max(contours, key=cv2.contourArea)
self._image_display.display_debug_contours('[OpenCvPlayAreaFinder] contours', image, contours, [contour_table])
self._play_area = Rectangle(*cv2.boundingRect(contour_table))
@staticmethod
def _approximate_contour(contour: np.ndarray) -> np.ndarray:
epsilon = 0.05 * cv2.arcLength(contour, True)
return cv2.approxPolyDP(contour, epsilon, True)
@staticmethod
def _does_contour_fit_area(contour: np.ndarray) -> bool:
area = Rectangle(*cv2.boundingRect(contour))
try:
# The table is 231cm * 111cm, which gives it a width/height ratio of 2.08
does_ratio_fit = 1.5 < area.width_height_ratio < 2.5
return does_ratio_fit
except ValueError:
return False
def __init__(self) -> None:
self._goal: Rectangle = None
self._orientation: Angle = None
self._play_area_finder = CvPlayAreaFinder()
self._image_display = CvImageDisplay()
class CvGoalFinder(IGoalFinder):
def __init__(self) -> None:
self._goal: Rectangle = None
self._orientation: Angle = None
self._play_area_finder = CvPlayAreaFinder()
self._image_display = CvImageDisplay()
def find(self, image: Image) -> Tuple[Rectangle, Angle]:
play_area = self._play_area_finder.find(image)
image.crop(play_area).process(self._process)
self._goal = Rectangle(
self._goal.top_left_corner.x + play_area.top_left_corner.x,
self._goal.top_left_corner.y + play_area.top_left_corner.y,
self._goal.width, self._goal.height)
return self._goal, self._orientation
def _process(self, image: np.ndarray) -> None:
grey = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
self._image_display.display_debug_image('[CvGoalFinder] grey', grey)
canny = cv2.Canny(grey, 100, 200)
self._image_display.display_debug_image('[CvGoalFinder] canny', canny)
contours, _ = cv2.findContours(canny, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
contours = [CvGoalFinder._approximate_contour(c) for c in contours]
contours = [
c for c in contours if CvGoalFinder._does_contour_fit_goal(c)
]
if len(contours) == 0:
raise GoalCouldNotBeFound
goal_contour = CvGoalFinder._get_brightest_area(grey, contours)
self._goal = Rectangle(*cv2.boundingRect(goal_contour))
image_height, image_width, _ = image.shape
self._compute_orientation(image_width, image_height)
self._image_display.display_debug_contours(
'[CvGoalFinder] goal_contour', image, contours, [goal_contour])
@staticmethod
def _does_contour_fit_goal(contour: np.ndarray) -> bool:
is_contour_rectangle = len(contour) == 4
rectangle = Rectangle(*cv2.boundingRect(contour))
# goal area is 27cm * 7.5cm, which gives a width/height ratio of 3.6 or 0.27
ratio = rectangle.width_height_ratio
does_ratio_fit = 2.6 < ratio < 4.6 or 2.6 < (1.0 / ratio) < 4.6
# From experimentation, we know that the goal has an area of around 1650 pixels
does_area_fit = 650 < rectangle.area < 2650
return is_contour_rectangle and does_ratio_fit and does_area_fit
@staticmethod
def _approximate_contour(contour: np.ndarray) -> np.ndarray:
epsilon = 0.05 * cv2.arcLength(contour, True)
return cv2.approxPolyDP(contour, epsilon, True)
@staticmethod
def _get_brightest_area(grey: np.ndarray,
contours: List[np.ndarray]) -> np.ndarray:
highest_mean_value = -1
brightest_area_contour = None
for contour in contours:
mask = np.zeros(grey.shape, np.uint8)
cv2.drawContours(mask, [contour], 0, 255, -1)
current_mean_value, _, _, _ = cv2.mean(grey, mask=mask)
if current_mean_value > highest_mean_value:
highest_mean_value = current_mean_value
brightest_area_contour = contour
return brightest_area_contour
def _compute_orientation(self, image_width, image_height) -> None:
goal_center = self._goal.get_center()
if self._goal.width_height_ratio > 1.0: # target is horizontal
if goal_center.y > image_height / 2: # target is on bottom
self._orientation = Angle(pi)
else:
self._orientation = Angle(0)
else: # target is vertical
if goal_center.x > image_width / 2: # target is on the right
self._orientation = Angle(pi / 2)
else:
self._orientation = Angle(3 * pi / 2)
def __init__(self) -> None:
self._aruco_dictionary = aruco.Dictionary_get(aruco.DICT_6X6_250)
self._obstacles_id = 0
self._detection_parameter = aruco.DetectorParameters_create()
self._obstacles: List[Rectangle] = []
self._image_display = CvImageDisplay()
def __init__(self, camera_id: int) -> None:
self.image_display = CvImageDisplay()
self._video_capture = cv2.VideoCapture(camera_id)
if not self._video_capture.isOpened():
raise CameraDoesNotExistError(camera_id)
class CvCameraCalibration(ICameraCalibration):
def __init__(self, camera_matrix: np.ndarray,
distortion_coefficients: np.ndarray,
play_area_finder: IPlayAreaFinder, image: Image) -> None:
self._play_area_finder = play_area_finder
self._image_display = CvImageDisplay()
self._camera_matrix = camera_matrix
self._distortion_coefficients = distortion_coefficients
self._optimized_camera_matrix, self._region_of_interest = \
cv2.getOptimalNewCameraMatrix(self._camera_matrix, self._distortion_coefficients,
image.size, 1, image.size)
self._camera_matrix_inverse = np.linalg.inv(
self._optimized_camera_matrix)
self._focal_x: float = self._optimized_camera_matrix[0, 0]
self._focal_y: float = self._optimized_camera_matrix[1, 1]
self._compute_distances(image)
def rectify_image(self, image: Image) -> Image:
rectified_image = image.process(self._process_rectify)
self._image_display.display_debug_image(
'[OpenCvCameraCalibration] rectified_image',
rectified_image.content)
return rectified_image
def _process_rectify(self, image: np.ndarray) -> np.ndarray:
rectified_image = cv2.undistort(image, self._camera_matrix,
self._distortion_coefficients, None,
self._optimized_camera_matrix)
region_of_interest_x, region_of_interest_y, roi_width, roi_height = self._region_of_interest
return rectified_image[region_of_interest_y:region_of_interest_y +
roi_height,
region_of_interest_x:region_of_interest_x +
roi_width, :]
def _compute_distances(self, image: Image) -> None:
table_roi = self._play_area_finder.find(image)
table_distance_x = (table_width_mm * self._focal_x) / table_roi.width
table_distance_y = (table_height_mm * self._focal_y) / table_roi.height
self._table_distance = (table_distance_x + table_distance_y) / 2
self._obstacle_distance = self._table_distance - obstacle_height_mm
self._robot_distance = self._table_distance - robot_height_mm
self._table_real_origin = self._convert_pixel_to_real(
table_roi.top_left_corner, self._table_distance)
def convert_table_pixel_to_real(self, pixel: Coord) -> Coord:
return self._convert_object_pixel_to_real(pixel, self._table_distance)
def convert_obstacle_pixel_to_real(self, pixel: Coord) -> Coord:
return self._convert_object_pixel_to_real(pixel,
self._obstacle_distance)
def convert_robot_pixel_to_real(self, pixel: Coord) -> Coord:
return self._convert_object_pixel_to_real(pixel, self._robot_distance)
def _convert_pixel_to_real(self, pixel: Coord, distance: float) -> Coord:
pixel_vector = np.array([pixel.x, pixel.y, 1]).transpose()
real_vector = self._camera_matrix_inverse.dot(pixel_vector)
real_vector = np.multiply(real_vector, distance).transpose()
return Coord(int(real_vector[0]), int(real_vector[1]))
def _adjust_real_to_table(self, real: Coord) -> Coord:
return Coord(real.x - self._table_real_origin.x,
real.y - self._table_real_origin.y)
def _convert_object_pixel_to_real(self, pixel: Coord,
distance: float) -> Coord:
real = self._convert_pixel_to_real(pixel, distance)
return self._adjust_real_to_table(real)
def __init__(self) -> None:
self._play_area: Rectangle = None
self._image_display = CvImageDisplay()
class CvRobotFinder(IRobotFinder):
def __init__(self) -> None:
self._aruco_dictionary = aruco.Dictionary_get(aruco.DICT_6X6_250)
self._top_left_id = 2
self._top_right_id = 3
self._bottom_left_id = 4
self._bottom_right_id = 5
self._ratio_distance_marker_size = 0.1
self._detection_parameter = aruco.DetectorParameters_create()
self._robot: Coord = None
self._orientation: Angle = None
self._image_display = CvImageDisplay()
def find(self, image: Image) -> Tuple[Coord, Angle]:
image.process(self._process)
return self._robot, self._orientation
def _process(self, image: np.ndarray) -> None:
grey = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
_, threshold = cv2.threshold(grey, 0, 255,
cv2.THRESH_BINARY + cv2.THRESH_OTSU)
self._image_display.display_debug_image('[CvRobotFinder] threshold',
threshold)
corners, ids, _ = aruco.detectMarkers(
threshold,
self._aruco_dictionary,
parameters=self._detection_parameter)
self._image_display.display_debug_aruco_markers(
'[CvRobotFinder] markers', image, corners, ids)
if ids is None:
raise RobotCouldNotBeFound
robot_found = False
i = 0
while not robot_found and i < ids.shape[0]:
if ids[i] == self._top_left_id:
self._robot = self._interpolate_robot_position_from_corners(
corners[i], 2, 1, 3)
self._orientation = CvRobotFinder._compute_robot_orientation(
corners[i])
robot_found = True
elif ids[i] == self._top_right_id:
self._robot = self._interpolate_robot_position_from_corners(
corners[i], 3, 0, 2)
self._orientation = CvRobotFinder._compute_robot_orientation(
corners[i])
robot_found = True
elif ids[i] == self._bottom_left_id:
self._robot = self._interpolate_robot_position_from_corners(
corners[i], 1, 0, 2)
self._orientation = CvRobotFinder._compute_robot_orientation(
corners[i])
robot_found = True
elif ids[i] == self._bottom_right_id:
self._robot = self._interpolate_robot_position_from_corners(
corners[i], 0, 3, 1)
self._orientation = CvRobotFinder._compute_robot_orientation(
corners[i])
robot_found = True
i += 1
if not robot_found:
raise RobotCouldNotBeFound
def _interpolate_robot_position_from_corners(
self, corners: np.ndarray, corner_to_adjust_index: int,
pred_corner_index: int, next_corner_index: int) -> Coord:
x_to_adjust, y_to_adjust = CvRobotFinder._extract_corner_coordination(
corners, corner_to_adjust_index)
pred_x, pred_y = CvRobotFinder._extract_corner_coordination(
corners, pred_corner_index)
next_x, next_y = CvRobotFinder._extract_corner_coordination(
corners, next_corner_index)
x = x_to_adjust + self._ratio_distance_marker_size * (
x_to_adjust -
pred_x) + self._ratio_distance_marker_size * (x_to_adjust - next_x)
y = y_to_adjust + self._ratio_distance_marker_size * (
y_to_adjust -
pred_y) + self._ratio_distance_marker_size * (y_to_adjust - next_y)
return Coord(int(x), int(y))
@staticmethod
def _compute_marker_bounding_rectangle(corners: np.ndarray) -> Rectangle:
min_x = np.min(corners[0, :, 0])
max_x = np.max(corners[0, :, 0])
min_y = np.min(corners[0, :, 1])
max_y = np.max(corners[0, :, 1])
return Rectangle(min_x, min_y, max_x - min_x, max_y - min_y)
@staticmethod
def _compute_robot_orientation(corners: np.ndarray) -> Angle:
return CvRobotFinder._compute_orientation_between_corners(
corners, 3, 0)
@staticmethod
def _compute_orientation_between_corners(corners: np.ndarray,
first_index: int,
second_index: int) -> Angle:
fourth_x, fourth_y = CvRobotFinder._extract_corner_coordination(
corners, first_index)
first_x, first_y = CvRobotFinder._extract_corner_coordination(
corners, second_index)
return Angle(atan2(fourth_y - first_y, fourth_x - first_x) - (pi / 2))
@staticmethod
def _extract_corner_coordination(corners: np.ndarray,
corner_index: int) -> Tuple[int, int]:
x = corners[0, corner_index, 0]
y = corners[0, corner_index, 1]
return x, y
def __init__(self) -> None:
self._source: Rectangle = None
self._orientation: Angle = None
self._image_display = CvImageDisplay()
class CvSourceFinder(ISourceFinder):
def __init__(self) -> None:
self._source: Rectangle = None
self._orientation: Angle = None
self._image_display = CvImageDisplay()
def find(self, image: Image) -> Tuple[Rectangle, Angle]:
image.process(self._process)
return self._source, self._orientation
def _process(self, image: np.ndarray) -> None:
grey = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
self._image_display.display_debug_image('[CvSourceFinder] grey', grey)
_, threshold = cv2.threshold(grey, 0, 255,
cv2.THRESH_BINARY + cv2.THRESH_OTSU)
kernel = np.ones((5, 5), np.uint8)
threshold = cv2.morphologyEx(threshold, cv2.MORPH_OPEN, kernel)
self._image_display.display_debug_image('[CvSourceFinder] threshold',
threshold)
contour_with_source = self._compute_biggest_contour(threshold, False)
contour_without_source = self._compute_biggest_contour(threshold, True)
source_contour = self._compute_source_contour(grey,
contour_with_source,
contour_without_source)
self._image_display.display_debug_contours(
'[CvSourceFinder] contours', image,
[contour_with_source, contour_without_source], [source_contour])
self._source = Rectangle(*cv2.boundingRect(source_contour))
image_height, image_width, _ = image.shape
self._compute_orientation(image_width, image_height)
return None
def _compute_source_contour(self, grey: np.ndarray, contour_with_source,
contour_without_source):
mask = np.zeros((grey.shape[0], grey.shape[1], 1), np.uint8)
cv2.drawContours(mask, [contour_without_source], 0, 255, -1)
cv2.drawContours(mask, [contour_with_source], 0, 0, -1)
kernel = np.ones((5, 5), np.uint8)
mask = cv2.erode(mask, kernel, iterations=1)
self._image_display.display_debug_image('[CvSourceFinder] source mask',
mask)
contours, _ = cv2.findContours(mask, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
contours = [CvSourceFinder._approximate_contour(c) for c in contours]
contours = [
c for c in contours if CvSourceFinder._does_contour_fit_source(c)
]
if len(contours) == 0:
raise SourceCouldNotBeFound
source_mean_value = 256
source_contour = None
for contour in contours:
mask = np.zeros(grey.shape, np.uint8)
cv2.drawContours(mask, [contour], 0, 255, -1)
current_mean_value, _, _, _ = cv2.mean(grey, mask=mask)
if current_mean_value < source_mean_value:
source_mean_value = current_mean_value
source_contour = contour
return source_contour
def _compute_biggest_contour(self, image: np.ndarray, approximate):
contours, _ = cv2.findContours(image, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
if len(contours) == 0:
raise SourceCouldNotBeFound
biggest_contour = max(contours, key=cv2.contourArea)
self._image_display.display_debug_contours(
'[CvSourceFinder] _compute_biggest_contour', image, contours,
[biggest_contour])
if approximate:
biggest_contour = CvSourceFinder._approximate_contour(
biggest_contour)
return biggest_contour
@staticmethod
def _approximate_contour(contour: np.ndarray) -> np.ndarray:
epsilon = 0.05 * cv2.arcLength(contour, True)
return cv2.approxPolyDP(contour, epsilon, True)
@staticmethod
def _does_contour_fit_source(contour: np.ndarray) -> bool:
is_rectangle = len(contour) == 4
if is_rectangle:
rectangle = Rectangle(*cv2.boundingRect(contour))
# From experimentation, we know that the goal has an area of around 1650 pixels
does_area_fit = 450 < rectangle.area < 2850
return does_area_fit
else:
return False
def _compute_orientation(self, image_width, image_height) -> None:
goal_center = self._source.get_center()
if self._source.width_height_ratio > 1.0: # target is horizontal
if goal_center.y > image_height / 2: # target is on bottom
self._orientation = Angle(pi)
else:
self._orientation = Angle(0)
else: # target is vertical
if goal_center.x > image_width / 2: # target is on the right
self._orientation = Angle(pi / 2)
else:
self._orientation = Angle(3 * pi / 2)