From 993d4c01416de81d9cccabff86c9721387bec918 Mon Sep 17 00:00:00 2001 From: Simon Pirkelmann Date: Sun, 8 Nov 2020 16:29:48 +0100 Subject: [PATCH] added options to run detection on inverted markers and to disable auto-exposure --- remote_control/aruco_estimator.py | 66 ++++++++++++++++++++----------- 1 file changed, 43 insertions(+), 23 deletions(-) diff --git a/remote_control/aruco_estimator.py b/remote_control/aruco_estimator.py index f312af2..6a3bc30 100644 --- a/remote_control/aruco_estimator.py +++ b/remote_control/aruco_estimator.py @@ -9,7 +9,6 @@ from queue import Queue import aruco - class ArucoEstimator: corner_marker_ids = { 'a': 0, @@ -25,8 +24,7 @@ class ArucoEstimator: 'd': {'pixel_coordinate': None, 'x': None, 'y': None, 'n_estimates': 0}, } - def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=8, grid_rows=8, - draw_marker_coordinate_system=False): + def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=8, grid_rows=8): self.grid_columns = grid_columns self.grid_rows = grid_rows @@ -36,21 +34,24 @@ class ArucoEstimator: self.robot_marker_estimates = dict([(marker_id, {'t': None, 'x': None, 'y': None, 'angle': None}) for marker_id in self.robot_marker_ids]) + self.draw_grid = False + self.event_queue = Queue() - self.draw_marker_coordinate_system = draw_marker_coordinate_system if use_realsense: # check if realsense camera is connected # Configure depth and color streams 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) # Start streaming self.pipeline.start(config) + # disable auto exposure + color_sensor = self.pipeline.get_active_profile().get_device().query_sensors()[1] + color_sensor.set_option(rs.option.enable_auto_exposure, False) + camera_intrinsics = self.pipeline.get_active_profile().get_stream( rs.stream.color).as_video_stream_profile().get_intrinsics() self.camera_matrix = np.zeros((3, 3)) @@ -68,10 +69,10 @@ class ArucoEstimator: # create detector and get parameters self.detector = aruco.MarkerDetector() # self.detector.setDictionary('ARUCO_MIP_36h12') - # self.detector.setDictionary('ARUCO_MIP_16h3') + #self.detector.setDictionary('ARUCO_MIP_16h3') # self.detector.setDictionary('ARUCO') - # self.detector.setDetectionMode(aruco.DM_NORMAL, 0.05) - # self.detector.setDetectionMode(aruco.DM_VIDEO_FAST, 0.05) + #self.detector.setDetectionMode(aruco.DM_NORMAL, 0.05) + self.detector.setDetectionMode(aruco.DM_VIDEO_FAST, 0.05) self.detector_params = self.detector.getParameters() @@ -82,7 +83,11 @@ class ArucoEstimator: print("\t{} : {}".format(val, self.detector_params.__getattribute__(val))) self.camparam = aruco.CameraParameters() - self.camparam.readFromXMLFile(os.path.join(os.path.dirname(__file__), "dfk72_6mm_param2.yml")) + if use_realsense: + self.camparam.readFromXMLFile(os.path.join(os.path.dirname(__file__), "realsense.yml")) + else: + self.camparam.readFromXMLFile(os.path.join(os.path.dirname(__file__), "dfk72_6mm_param2.yml")) + print(self.camparam) def mouse_callback(self, event, px, py, flags, param): """ @@ -120,7 +125,7 @@ class ArucoEstimator: else: print("not all markers have been detected!") - def run_tracking(self): + def run_tracking(self, draw_markers=True, draw_marker_coordinate_system=False, invert_grayscale=False): """ Run the marker tracking in a loop """ @@ -143,6 +148,8 @@ class ArucoEstimator: t_image = time.time() gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) + if invert_grayscale: + cv2.bitwise_not(gray, gray) # run aruco marker detection detected_markers = self.detector.detect(gray) @@ -151,7 +158,7 @@ class ArucoEstimator: detected_marker_data = {} for marker in detected_markers: detected_marker_data[marker.id] = {'marker_center': marker.getCenter()} - if marker.id > 0: + if marker.id >= 0: if marker.id in self.corner_marker_ids.values(): marker.calculateExtrinsics(0.1, self.camparam) else: @@ -159,10 +166,11 @@ class ArucoEstimator: detected_marker_data[marker.id]['Rvec'] = marker.Rvec detected_marker_data[marker.id]['Tvec'] = marker.Tvec - if marker.id > 0: # draw markers onto the image - marker.draw(color_image, np.array([255, 255, 255]), 2, True) + if marker.id >= 0: # draw markers onto the image + if draw_markers: + marker.draw(color_image, np.array([255, 255, 255]), 2, True) - if self.draw_marker_coordinate_system: + if draw_marker_coordinate_system: aruco.CvDrawingUtils.draw3dAxis(color_image, self.camparam, marker.Rvec, marker.Tvec, .1) # store data @@ -170,7 +178,8 @@ class ArucoEstimator: self.update_estimate(marker_id, data['marker_center'], data['Rvec'], data['Tvec'], t_image) # draw grid - color_image = self.draw_grid_lines(color_image, detected_marker_data) + if self.draw_grid: + color_image = self.draw_grid_lines(color_image, detected_marker_data) color_image = self.draw_robot_pos(color_image, detected_marker_data) # Show images @@ -179,8 +188,18 @@ class ArucoEstimator: if key > 0: self.event_queue.put(('key', key)) + if key == ord('g'): + self.draw_grid = not self.draw_grid if key == ord('q'): running = False + if key == ord('a'): + color_sensor = self.pipeline.get_active_profile().get_device().query_sensors()[1] + if color_sensor.get_option(rs.option.enable_auto_exposure) == 1.0: + color_sensor.set_option(rs.option.enable_auto_exposure, False) + print("auto exposure OFF") + else: + color_sensor.set_option(rs.option.enable_auto_exposure, True) + print("auto exposure ON") finally: cv2.destroyAllWindows() if self.pipeline is not None: @@ -309,7 +328,7 @@ class ArucoEstimator: corner_1_center = self.corner_estimates[corner_1]['pixel_coordinate'] corner_2_center = self.corner_estimates[corner_2]['pixel_coordinate'] if corner_1_center is not None and corner_2_center is not None: - frame = cv2.line(frame, corner_1_center, corner_2_center, color=(0, 0, 255), thickness=2) + frame = cv2.line(frame, tuple(corner_1_center), tuple(corner_2_center), color=(0, 0, 255), thickness=2) return frame def draw_grid_lines(self, frame, detected_marker_data): @@ -364,17 +383,18 @@ class ArucoEstimator: robot_corners_pixel_coords = {} for marker_id, estimate in self.robot_marker_estimates.items(): if marker_id in detected_marker_data.keys(): - robot_corners_pixel_coords[marker_id] = tuple(detected_marker_data[marker_id]['center']) + robot_corners_pixel_coords[marker_id] = tuple(detected_marker_data[marker_id]['marker_center']) for marker_id, coord in robot_corners_pixel_coords.items(): x = self.robot_marker_estimates[marker_id]['x'] y = self.robot_marker_estimates[marker_id]['y'] angle = self.robot_marker_estimates[marker_id]['angle'] - frame = cv2.putText(frame, "pos = ({:5.3f}, {:5.3f}), ang = {:5.3f}".format(x, y, angle), coord, - cv2.FONT_HERSHEY_SIMPLEX, 0.50, (0, 255, 0)) + #frame = cv2.putText(frame, "pos = ({:5.3f}, {:5.3f}), ang = {:5.3f}".format(x, y, angle), coord, + # cv2.FONT_HERSHEY_SIMPLEX, 0.50, (0, 255, 0)) + frame = cv2.putText(frame, f"{marker_id}", coord, cv2.FONT_HERSHEY_PLAIN, 2, (0, 255, 255), thickness=4) return frame if __name__ == "__main__": - estimator = ArucoEstimator(use_realsense=False, draw_marker_coordinate_system=True) - estimator.run_tracking() + estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[11, 12, 13, 14]) + estimator.run_tracking(draw_markers=True, invert_grayscale=True)