Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
    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)
Ejemplo n.º 3
0
 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])
Ejemplo n.º 4
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