diff --git a/remote_control/aruco_estimator.py b/remote_control/aruco_estimator.py index de20d6c..531489c 100644 --- a/remote_control/aruco_estimator.py +++ b/remote_control/aruco_estimator.py @@ -36,7 +36,6 @@ class ArucoEstimator: self.robot_marker_estimates = dict([(id, None) for id in self.robot_marker_ids]) self.event_queue = Queue() - self.draw_marker_coordinate_system = draw_marker_coordinate_system if use_realsense: # check if realsense camera is connected @@ -85,7 +84,15 @@ class ArucoEstimator: self.camparam.readFromXMLFile(os.path.join(os.path.dirname(__file__), "dfk72_6mm_param2.yml")) def mouse_callback(self, event, px, py, flags, param): - if event == 1: + """ + This function is called for each mouse event inside the opencv window. + If there are reference markers available we compute the real world position corresponding to the clicked + position and put it in an event queue. + :param event: type of event (mouse movement, left click, right click, etc.) + :param px: x-position of event + :param py: y-position of event + """ + if event == cv2.EVENT_LBUTTONDOWN: if self.all_corners_detected(): # inter/extrapolate from clicked point to marker position px1 = self.corner_estimates['a']['pixel_coordinate'][0] @@ -113,6 +120,9 @@ class ArucoEstimator: print("not all markers have been detected!") def run_tracking(self): + """ + Run the marker tracking in a loop + """ cv2.namedWindow('RoboRally', cv2.WINDOW_AUTOSIZE) cv2.setMouseCallback('RoboRally', self.mouse_callback) @@ -139,7 +149,7 @@ class ArucoEstimator: # extract data for detected markers detected_marker_data = {} for marker in detected_markers: - detected_marker_data[marker.id] = {'center': marker.getCenter()} + detected_marker_data[marker.id] = {'marker_center': marker.getCenter()} if marker.id > 0: if marker.id in self.corner_marker_ids.values(): marker.calculateExtrinsics(0.1, self.camparam) @@ -156,7 +166,7 @@ class ArucoEstimator: # store data for id, data in detected_marker_data.items(): - self.update_estimate(id, data['center'], data['Rvec'], data['Tvec'], t_image) + self.update_estimate(id, data['marker_center'], data['Rvec'], data['Tvec'], t_image) # draw grid color_image = self.draw_grid_lines(color_image, detected_marker_data) @@ -176,12 +186,14 @@ class ArucoEstimator: # Stop streaming self.pipeline.stop() - def update_estimate(self, id, pixel_coord, rvec, tvec, t_image): + def update_estimate(self, id, pixel_coord_center, rvec, tvec, t_image): # update the marker estimate with new data if id in self.corner_marker_ids.values(): # for corner markers we compute the mean of all measurements s.t. the position stabilizes over time # (we assume the markers do not move) - corner = next(filter(lambda key: self.corner_marker_ids[key] == id, self.corner_marker_ids.keys())) # get corresponding corner to the detected marker + + # get corresponding corner to the detected marker + corner = next(filter(lambda key: self.corner_marker_ids[key] == id, self.corner_marker_ids.keys())) old_estimate_x = self.corner_estimates[corner]['x'] old_estimate_y = self.corner_estimates[corner]['y'] n_estimates = self.corner_estimates[corner]['n_estimates'] @@ -198,7 +210,7 @@ class ArucoEstimator: self.corner_estimates[corner]['x'] = new_estimate_x self.corner_estimates[corner]['y'] = new_estimate_y self.corner_estimates[corner]['n_estimates'] = n_estimates + 1 - self.corner_estimates[corner]['pixel_coordinate'] = pixel_coord + self.corner_estimates[corner]['pixel_coordinate'] = pixel_coord_center elif id in self.robot_marker_ids: # for robot markers we extract x and y position as well as the angle @@ -292,32 +304,29 @@ class ArucoEstimator: if value['n_estimates'] != 0: print(f"corner marker {key} at pos ({value['x']},{value['y']})") - def draw_corner_line(self, frame, corner_1, corner_2, corner_coords_dict): + def draw_corner_line(self, frame, corner_1, corner_2): # draws a line between the given markers onto the given frame - if corner_1 in corner_coords_dict and corner_2 in corner_coords_dict: - frame = cv2.line(frame, corner_coords_dict[corner_1], corner_coords_dict[corner_2], color=(0, 0, 255), - thickness=2) + 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) return frame def draw_grid_lines(self, frame, detected_marker_data): # draws a grid onto the given frame - board_corners_pixel_coords = {} - for corner, id in self.corner_marker_ids.items(): - if id in detected_marker_data.keys(): - board_corners_pixel_coords[corner] = tuple(detected_marker_data[id]['center']) + frame = self.draw_corner_line(frame, 'a', 'b') + frame = self.draw_corner_line(frame, 'b', 'c') + frame = self.draw_corner_line(frame, 'c', 'd') + frame = self.draw_corner_line(frame, 'd', 'a') - frame = self.draw_corner_line(frame, 'a', 'b', board_corners_pixel_coords) - frame = self.draw_corner_line(frame, 'b', 'c', board_corners_pixel_coords) - frame = self.draw_corner_line(frame, 'c', 'd', board_corners_pixel_coords) - frame = self.draw_corner_line(frame, 'd', 'a', board_corners_pixel_coords) - - if set(board_corners_pixel_coords.keys()) == set(self.corner_marker_ids.keys()): # all markers have been detected - # compute column line - a = np.array(board_corners_pixel_coords['a']) - b = np.array(board_corners_pixel_coords['b']) - c = np.array(board_corners_pixel_coords['c']) - d = np.array(board_corners_pixel_coords['d']) + if not any([estimate['pixel_coordinate'] is None for estimate in self.corner_estimates.values()]): + # compute outlines of board + a = self.corner_estimates['a']['pixel_coordinate'] + b = self.corner_estimates['b']['pixel_coordinate'] + c = self.corner_estimates['c']['pixel_coordinate'] + d = self.corner_estimates['d']['pixel_coordinate'] + # draw vertical lines vab = b - a vdc = c - d for x in range(1,self.grid_columns): @@ -326,6 +335,7 @@ class ArucoEstimator: frame = cv2.line(frame, tuple(column_line_top), tuple(column_line_bottom), color=(0, 255, 0), thickness=1) + # draw horizontal lines vad = d - a vbc = c - b for y in range(1, self.grid_rows): @@ -359,10 +369,11 @@ class ArucoEstimator: x = self.robot_marker_estimates[id]['x'] y = self.robot_marker_estimates[id]['y'] angle = self.robot_marker_estimates[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)) return frame if __name__ == "__main__": - estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[15], draw_marker_coordinate_system=True) + estimator = ArucoEstimator(use_realsense=False, draw_marker_coordinate_system=True) estimator.run_tracking()