def __init__(self,
id,
corners,
x_pos=None,
y_pos=None,
role="attacker",
path_eps=25,
angle_eps=15,
vision_dist=100):
Marker.__init__(self, id, corners, x_pos, y_pos)
self.front_side = None
self.back_side = None
self.left_side = None
self.right_side = None
self.role = role
self.path = Path([])
self.path_eps = path_eps
self.angle_eps = angle_eps
self.path_created = False
self.actual_point = None
self.angle_to_actual_point = None
self.angles_list = []
self.movement = False
self.rotation = False
self.stop_riding = False
self.vision_dist = vision_dist
def __init__(self, id, corners, x_pos=None, y_pos=None):
Marker.__init__(self, id, corners, x_pos, y_pos)
self.center_smoother = PointSmoother()
self.corners_smoother = CornersSmoother()
self.front_side = None
self.back_side = None
self.left_side = None
self.right_side = None
self.marker_size = rosparam.get_param("marker_size") * 10
self.dist_between_markers_pair_centers = rosparam.get_param("dist_between_markers_pair_centers") * 10
self.corners_list = []
def find_empty_right_marker(self, left_marker, pix_dist_between_markers):
marker_w_top_dx = left_marker.corners[1].x - left_marker.corners[0].x
marker_w_bot_dx = left_marker.corners[2].x - left_marker.corners[3].x
w_dx = np.mean([marker_w_bot_dx, marker_w_top_dx])
marker_h_left_dy = left_marker.corners[1].y - left_marker.corners[0].y
marker_h_right_dy = left_marker.corners[2].y - left_marker.corners[3].y
h_dy = np.mean([marker_h_left_dy, marker_h_right_dy])
w1 = get_distance_between_points(left_marker.corners[0],
left_marker.corners[1])
w2 = get_distance_between_points(left_marker.corners[3],
left_marker.corners[2])
cos_a1, cos_a2 = float(w_dx) / float(w1), float(w_dx) / float(w2)
sin_a1, sin_a2 = float(h_dy) / float(w1), float(h_dy) / float(w2)
cos_a = np.mean([cos_a1, cos_a2])
sin_a = np.mean([sin_a1, sin_a2])
dist_dx = pix_dist_between_markers * cos_a
dist_dy = pix_dist_between_markers * sin_a
corner0 = (left_marker.corners[1].x + dist_dx,
left_marker.corners[1].y + dist_dy)
corner3 = (left_marker.corners[2].x + dist_dx,
left_marker.corners[2].y + dist_dy)
corner1 = (left_marker.corners[1].x + dist_dx + w_dx,
left_marker.corners[1].y + dist_dy + h_dy)
corner2 = (left_marker.corners[2].x + dist_dx + w_dx,
left_marker.corners[2].y + dist_dy + h_dy)
corners = [corner0, corner1, corner2, corner3]
return Marker(left_marker.id, corners)
def estimate_pose_duplicates(self, duplicates, dx=0, dy=0):
for duplicates_id in duplicates.keys():
x, y = [], []
x_dir, y_dir = [], []
x0, y0, x1, y1, x2, y2, x3, y3 = [], [], [], [], [], [], [], []
for marker in duplicates[duplicates_id]:
fields_marker = self.estimate_pose_single_marker(
marker, dx, dy)
x.append(fields_marker.center.x)
y.append(fields_marker.center.y)
x_dir.append(fields_marker.direction.x)
y_dir.append(fields_marker.direction.y)
x0.append(marker.corners[0].x)
y0.append(marker.corners[0].y)
x1.append(marker.corners[1].x)
y1.append(marker.corners[1].y)
x2.append(marker.corners[2].x)
y2.append(marker.corners[2].y)
x3.append(marker.corners[3].x)
y3.append(marker.corners[3].y)
corners = [((np.mean(x0), np.mean(y0))),
((np.mean(x1), np.mean(y1))),
((np.mean(x2), np.mean(y2))),
((np.mean(x3), np.mean(y3)))]
marker = Marker(duplicates_id, corners)
self.markers_on_field[marker.id] = marker
def detect_markers(self, aruco_dict, aruco_params):
self.detected_ids = []
for camera in self.cameras_data:
camera.detected_markers = []
gray = cv2.cvtColor(camera.img, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters=aruco_params)
if ids is not None:
for id, c in zip(ids, corners):
if id[0] in self.valid_markers:
camera.detected_markers.append(Marker(id[0], c[0], camera.x_pos, camera.y_pos))
self.detected_ids.append(id[0])
SAVED = False
parametrs_dict = {}
while RUN:
for camera in cameras:
if camera.stream_is_ready():
img = camera.get_img()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(gray,
aruco_dict,
parameters=aruco_params)
if ids is not None:
markers = []
for i, c in zip(ids, corners):
markers.append(Marker(i[0], c[0]))
camera.set_detected_markers(markers)
aruco.drawDetectedMarkers(img, corners, ids)
camera.set_camera_position()
actual_markers = camera.get_actual_markers()
markers_centers = list(marker.center.to_list()
for marker in actual_markers)
if camera.set_k_points(markers_centers):
parametrs = camera.get_parametrs_dict()
parametrs_dict[str(camera.id)] = parametrs
if SAVED:
cv2.putText(img, "SAVED!", (20, 30), cv2.FONT_HERSHEY_COMPLEX,
0.8, (0, 255, 100), 3)
cv2.imshow(str(camera.id), img)
if cv2.waitKey(10) & 0xFF == 27:
RUN = not RUN