diff --git a/remote_control/opencv_viewer_example.py b/remote_control/opencv_viewer_example.py index aea563d..372fe12 100644 --- a/remote_control/opencv_viewer_example.py +++ b/remote_control/opencv_viewer_example.py @@ -38,7 +38,7 @@ class ArucoEstimator: self.pipeline = rs.pipeline() config = rs.config() # config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) - # config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30) + #config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) # config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) @@ -107,36 +107,30 @@ class ArucoEstimator: gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) - # aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250) - aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL) + #aruco_dict = aruco.Dictionary_get(aruco.DICT_APRILTAG_16H5) + #aruco_dict = aruco.Dictionary_get(aruco.DICT_7X7_50) + #aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) + aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_1000) + #aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL) # fast parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) frame = aruco.drawDetectedMarkers(color_image.copy(), corners, ids) if ids is not None: for id, corner in zip(ids, corners): + if id in self.corner_marker_ids.values(): + marker_size = 0.1 + else: + marker_size = 0.07 corner_pixel_coord = np.mean(corner[0], axis=0) - res = aruco.estimatePoseSingleMarkers(corner, 0.10, self.camera_matrix, self.dist_coeffs) - rvecs = res[0][0][0] - tvecs = res[1][0][0] - - self.update_estimate(id[0], corner_pixel_coord, rvecs, tvecs) + # res = aruco.estimatePoseSingleMarkers(corner, marker_size, self.camera_matrix, self.dist_coeffs) + # rvecs = res[0][0][0] + # tvecs = res[1][0][0] + # + # self.update_estimate(id[0], corner_pixel_coord, rvecs, tvecs) frame = self.draw_grid_lines(frame, corners, ids) frame = self.draw_robot_pos(frame, corners, ids) - else: - # pretent we detected some markers - pass - self.update_estimate(0, rvec=np.array([0, 0, 0]), tvec=np.array([-1, 1, 0])) - self.update_estimate(1, rvec=np.array([0, 0, 0]), tvec=np.array([1, 1.5, 0])) - self.update_estimate(2, rvec=np.array([0, 0, 0]), tvec=np.array([1, -1.5, 0])) - self.update_estimate(3, rvec=np.array([0, 0, 0]), tvec=np.array([-1, -1, 0])) - #import random - #self.update_estimate(12, rvec=np.array([0.0, 0.0, 0.0]), tvec=np.array([-1.0 + random.random() * 2, -1.0 + random.random() * 2, 0])) - self.update_estimate(12, rvec=np.array([0.0, 0.0, 0.0]), - tvec=np.array([1.2, 0.42, 0])) - self.update_estimate(13, rvec=np.array([0.0, 0.0, 0.0]), - tvec=np.array([-1.2, -0.42, 0])) # Show images cv2.imshow('RoboRally', frame) @@ -344,6 +338,6 @@ class ArucoEstimator: if __name__ == "__main__": - estimator = ArucoEstimator() + estimator = ArucoEstimator(use_realsense=False) estimator.run_tracking()