diff --git a/remote_control/aruco_estimator.py b/remote_control/aruco_estimator.py index 7c267d6..de20d6c 100644 --- a/remote_control/aruco_estimator.py +++ b/remote_control/aruco_estimator.py @@ -2,6 +2,7 @@ import pyrealsense2 as rs import numpy as np import cv2 import os +import time from shapely.geometry import LineString from queue import Queue @@ -10,7 +11,7 @@ import aruco class ArucoEstimator: corner_marker_ids = { - 'a': 15, + 'a': 0, 'b': 1, 'c': 2, 'd': 3 @@ -19,13 +20,13 @@ class ArucoEstimator: angles = [] corner_estimates = { - 'a': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, - 'b': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, - 'c': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, - 'd': {'pixel_coordinate': None, 'real_world_estimate': None, 'n_estimates': 0 }, + 'a': {'pixel_coordinate': None, 'x': None, 'y': None, 'n_estimates': 0 }, + 'b': {'pixel_coordinate': None, 'x': None, 'y': None, 'n_estimates': 0 }, + 'c': {'pixel_coordinate': None, 'x': None, 'y': None, 'n_estimates': 0 }, + '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): + def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=8, grid_rows=8, draw_marker_coordinate_system=False): self.grid_columns = grid_columns self.grid_rows = grid_rows @@ -36,6 +37,8 @@ class ArucoEstimator: 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() @@ -90,10 +93,10 @@ class ArucoEstimator: py1 = self.corner_estimates['a']['pixel_coordinate'][1] py3 = self.corner_estimates['c']['pixel_coordinate'][1] - x1 = self.corner_estimates['a']['real_world_estimate'][0] - x3 = self.corner_estimates['c']['real_world_estimate'][0] - y1 = self.corner_estimates['a']['real_world_estimate'][1] - y3 = self.corner_estimates['c']['real_world_estimate'][1] + x1 = self.corner_estimates['a']['x'] + x3 = self.corner_estimates['c']['x'] + y1 = self.corner_estimates['a']['y'] + y3 = self.corner_estimates['c']['y'] alpha = (px - px1) / (px3 - px1) beta = (py - py1) / (py3 - py1) @@ -126,6 +129,7 @@ class ArucoEstimator: else: # Capture frame-by-frame ret, color_image = self.cv_camera.read() + t_image = time.time() gray = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) @@ -146,11 +150,13 @@ class ArucoEstimator: if marker.id > 0: # draw markers onto the image marker.draw(color_image, np.array([255, 255, 255]), 2, True) - aruco.CvDrawingUtils.draw3dAxis(color_image, self.camparam, marker.Rvec, marker.Tvec, .1) + + if self.draw_marker_coordinate_system: + aruco.CvDrawingUtils.draw3dAxis(color_image, self.camparam, marker.Rvec, marker.Tvec, .1) # store data for id, data in detected_marker_data.items(): - self.update_estimate(id, data['center'], data['Rvec'], data['Tvec']) + self.update_estimate(id, data['center'], data['Rvec'], data['Tvec'], t_image) # draw grid color_image = self.draw_grid_lines(color_image, detected_marker_data) @@ -170,23 +176,27 @@ class ArucoEstimator: # Stop streaming self.pipeline.stop() - def update_estimate(self, id, pixel_coord, rvec, tvec): + def update_estimate(self, id, pixel_coord, 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 - old_estimate = self.corner_estimates[corner]['real_world_estimate'] + old_estimate_x = self.corner_estimates[corner]['x'] + old_estimate_y = self.corner_estimates[corner]['y'] n_estimates = self.corner_estimates[corner]['n_estimates'] x = tvec[0][0] y = -tvec[1][0] # flip y coordinate - tvec_proj = np.array([x, y]) - if old_estimate is not None: - new_estimate = (n_estimates * old_estimate + tvec_proj) / (n_estimates + 1) # weighted update + if not any([old_estimate_x is None, old_estimate_y is None]): + new_estimate_x = (n_estimates * old_estimate_x + x) / (n_estimates + 1) # weighted update + new_estimate_y = (n_estimates * old_estimate_y + y) / (n_estimates + 1) else: - new_estimate = tvec_proj # first estimate - self.corner_estimates[corner]['real_world_estimate'] = new_estimate + new_estimate_x = x # first estimate + new_estimate_y = y + self.corner_estimates[corner]['t'] = t_image + 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 @@ -203,7 +213,7 @@ class ArucoEstimator: angle = -euler_angles[2][0] * np.pi / 180.0 self.angles.append(angle) - self.robot_marker_estimates[id] = (x, y, angle) + self.robot_marker_estimates[id] = {'t': t_image, 'x': x, 'y': y, 'angle': angle} def all_corners_detected(self): # checks if all corner markers have been detected at least once @@ -227,10 +237,10 @@ class ArucoEstimator: assert self.all_corners_detected() # compute column line - a = self.corner_estimates['a']['real_world_estimate'] - b = self.corner_estimates['b']['real_world_estimate'] - c = self.corner_estimates['c']['real_world_estimate'] - d = self.corner_estimates['d']['real_world_estimate'] + a = np.array([self.corner_estimates['a']['x'], self.corner_estimates['a']['y']]) + b = np.array([self.corner_estimates['b']['x'], self.corner_estimates['b']['y']]) + c = np.array([self.corner_estimates['c']['x'], self.corner_estimates['c']['y']]) + d = np.array([self.corner_estimates['d']['x'], self.corner_estimates['d']['y']]) x_frac = (x + 0.5) / self.grid_columns y_frac = (y + 0.5) / self.grid_rows @@ -274,14 +284,13 @@ class ArucoEstimator: return point_of_intersection def get_grid_point_from_pos(self): - # return the nearest grid point for the given position estimate + # TODO return the nearest grid point for the given position estimate pass def print_corner_estimates(self): for key, value in self.corner_estimates.items(): if value['n_estimates'] != 0: - print("corner marker {} at pos {}".format(key, value['real_world_estimate'])) - print() + print(f"corner marker {key} at pos ({value['x']},{value['y']})") def draw_corner_line(self, frame, corner_1, corner_2, corner_coords_dict): # draws a line between the given markers onto the given frame @@ -330,7 +339,8 @@ class ArucoEstimator: def get_robot_state_estimate(self, id): if id in self.robot_marker_estimates: if self.robot_marker_estimates[id] is not None: - return np.array(self.robot_marker_estimates[id]) + return np.array([self.robot_marker_estimates[id]['x'], self.robot_marker_estimates[id]['y'], + self.robot_marker_estimates[id]['angle']]) else: print(f"error: no estimate available for robot {id}") return None @@ -346,13 +356,13 @@ class ArucoEstimator: robot_corners_pixel_coords[id] = tuple(detected_marker_data[id]['center']) for id, coord in robot_corners_pixel_coords.items(): - x = self.robot_marker_estimates[id][0] - y = self.robot_marker_estimates[id][1] - angle = self.robot_marker_estimates[id][2] + 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)) return frame if __name__ == "__main__": - estimator = ArucoEstimator(use_realsense=False) + estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[15], draw_marker_coordinate_system=True) estimator.run_tracking()